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Abstract 


Many  of  the  UAV  missions  require  them  to  fly  in  urban  setting  over  low  altitudes  making 
them  vulnerable  to  the  collision  with  objects  in  their  vicinity  such  as  trees,  buildings, 
power  lines  etc.  So  it  is  very  crucial  for  UAVs  to  have  autonomous  obstacle  detection  and 
avoidance  capability  to  complete  their  missions  safely  and  successfully.  In  order  to  find  a 
solution  to  this  problem,  first  an  elaborate  literature  survey  has  been  conducted.  This 
literature  survey  led  to  a  fairly  thorough  understanding  of  the  state-of-art  techniques  in 
the  area  of  reactive  obstacle  avoidance  algorithms  for  UAVs.  For  reactive  obstacle 
avoidance,  it  is  essential  to  sense  the  environment  around  UAV  closely  and  continuously 
with  some  on-board  sensor.  A  video  camera  is  very  popular  such  sensor.  It  is  a  2D 
passive  (vision)  sensor  and  has  many  advantages  such  as  compact  size,  low  weight,  low 
cost,  and  passive  sensing  capability.  However,  the  data  from  vision  sensor  is  usually 
noisy  and  inaccurate,  hence  must  be  filtered.  So  an  Extended  Kalman  Filter  (EKF)  has 
been  developed  to  get  the  best  possible  estimate  of  relative  position  of  the  obstacles  and 
target  with  respect  to  the  UAV  based  on  noisy  measurements  from  the  vision  sensor. 
After  getting  the  obstacle  position  estimate  from  EKF,  the  Collision  Cone  approach  has 
been  applied  to  detect  any  incoming  obstacle  and  to  find  a  new  aiming  point  in  order  to 
avoid  it.  Once  the  aiming  point  is  known,  the  obstacle  avoidance  problem  reduces  to  a 
guidance  problem.  Next,  two  recently  proposed  nonlinear  guidance  laws,  Nonlinear 
Geometric  Guidance  (NGG)  and  Differential  Geometric  Guidance  (DGG),  have  been 
applied  for  guidance  purpose.  This  paper  describes  the  implementation  of  the  guidance 
strategies  NGG  and  DGG  while  estimating  the  position  of  the  obstacles  with  the  noisy 
measurements  from  a  2D  passive  vision  sensor  with  EKF.  The  algorithm  developed  has 
been  validated  from  a  number  of  simulation  studies  in  three-dimensional  scenario. 
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Chapter  1 
Introduction 


Unmanned  aerial  vehicles  (UAVs)  are  expected  to  be  ubiquitous  in  the  near  future, 
autonomously  performing  complex  military  and  civilian  missions  such  as  reconnaissance, 
environmental  monitoring,  border  patrol,  search  and  rescue  operations,  disaster  relief, 
traffic  monitoring  etc  [1]. 

Many  of  these  applications  require  the  UAV  to  fly  at  low  altitudes  in  proximity  with 
man  made  and  natural  structures  such  as  buildings,  trees,  power  lines  etc.  A  collision 
with  such  structure  would  be  fatal  and  would  result  in  mission  failure  with  the  loss  of  the 
vehicle.  Therefore,  UAVs  should  have  some  sense  of  “situation  awareness”  [2]  in  order 
to  successfully  sense  and  avoid  obstacles.  At  the  same  time  they  should  be  able  to  go 
ahead  to  pursue  their  original  mission.  This  requires  robust  and  computationally  feasible 
obstacle  avoidance  algorithms  to  be  implemented  on-board  the  UAV.  However,  UAVs 
have  limited  computational  resources.  Following  limitations  exist  for  any  system  to  be 
implemented  onboard. 

1 .  Size,  weight  and  power  consumption  of  onboard  processor  is  limited.  The  processing 
power  and  memory  available  to  the  algorithm  are  also  limited.  Therefore,  the 
algorithm  must  be  computationally  very  efficient. 

2.  The  algorithm  must  respond  quickly,  because  it  needs  to  be  implemented  in  an  online 
navigation  system. 

3.  The  payload  of  UAV  is  extremely  limited.  Therefore  the  sensor  used  to  gain 
information  from  the  surroundings  must  be  lightweight.  Also  it  should  be  energy 
efficient  and  economical. 

4.  UAVs  may  need  to  avoid  detection  for  certain  military  missions  like  enemy 
reconnaissance.  Thus  active  sensors,  which  send  out  energy  into  the  environment,  are 
not  suitable. 
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In  view  of  limitations  (3)  and  (4),  vision  based  navigation  is  a  very  suitable  option.  It 
is  compact,  lightweight,  economical  as  well  as  a  passive  sensor.  Therefore  the  obstacle 
avoidance  algorithms  should  be  able  to  perform  well  with  vision  information.  However, 
the  infonnation  from  vision  sensor  is  usually  noisy  and  inaccurate.  Hence  algorithm  must 
filter  out  any  noise  or  at  least  minimize  noise  in  order  to  get  best  possible  estimate  of  the 
obstacle/target  position  so  that  reactive  obstacle  avoidance  can  work  properly.  At  the 
same  time  algorithm  must  be  computationally  efficient  to  satisfy  the  above  requirements 
as  well. 

UAV  obstacle  avoidance  approaches  can  be  classified  into  deliberative  global  path 
planning  and  reactive  local  obstacle  avoidance  algorithms.  Global  path  planning 
algorithms  assume  complete  knowledge  of  the  environment  beforehand  and  calculate  an 
obstacle-free  trajectory  to  the  goal  point.  These  methods  take  into  account  the  dynamics 
of  the  UAV,  and  avoid  known  obstacles  like  buildings,  trees  etc.  Additionally,  the  UAV 
constraints  such  as  turn  radius  and  kinodynamic  constraints  are  also  taken  into  account. 
Usually  they  attempt  to  find  optimal  paths.  However,  these  methods  are  computationally 
expensive  hence  cannot  be  implemented  online.  Also  they  require  complete  knowledge  of 
the  surrounding  environment  which  is  not  the  case  always.  Local  methods  are  reactive  in 
nature,  i.e.  an  onboard  sensor  detects  the  possibility  of  collision  and  then  an  alternative 
route  is  planned  online.  These  consist  of  computationally  efficient  algorithms  and  are 
able  to  react  quickly  to  changes  in  the  environment.  However  these  methods  cannot 
guarantee  optimality.  Local  navigation  methods  do  not  retain  global  information  and 
continually  react  to  changing  enviromnents.  It  is  essential  that  local  planning  methods  are 
fast  and  reliable,  since  the  speed  and  reliability  of  the  UAV  is  limited  by  them. 

Recent  obstacle  avoidance  approaches  implement  multi-layer  architectures.  A  global 
planning  layer  first  finds  a  dynamically  feasible  obstacle-free  optimal  path  to  the  goal, 
and  then  a  local  obstacle  avoidance  layer  reacts  to  changes  in  the  environment  and 
computes  an  alternate,  obstacle-free  path  online.  This  scheme  reduces  the  computation 
resources  required  since  the  amount  of  online  computation  carried  out  is  much  less. 
Optimality  of  the  path  may  also  be  guaranteed,  except  for  brief  periods  when  the  UAV 
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maneuvers  away  from  the  planned  trajectory  to  avoid  a  local  obstacle.  This  paper  reviews 
some  recently  developed  ideas  of  reactive  obstacle  avoidance  algorithms.  A  literature 
survey  of  evolving  philosophies  on  obstacle  avoidance  of  UAVs  is  presented  in  Chapter 
2. 


Obstacle  avoidance  is  often  considered  as  another  side  of  the  target  interception 
problem  [3].  This  is  because  once  an  incoming  obstacle  is  detected,  an  alternate  aiming 
point  must  be  found  in  order  to  avoid  it.  After  this,  the  problem  reduces  to  one  of  finding 
a  path  which  satisfies  terminal  conditions.  This  constitutes  a  guidance  problem.  The 
problem  considered  in  this  paper  involves  collision  avoidance  with  stationary  obstacles  as 
they  appear  during  flight.  A  goal  point  is  considered  after  the  obstacle  region  (which  may 
be  a  way-point  decided  by  a  global  planner).  The  objective  is  to  avoid  obstacles  along  the 
way  and  then  reach  the  goal  point. 

The  developed  algorithm  here,  applies  vision  based  sensor  (video  camera)  to  sense 
the  surrounding  environment  and  detect  any  incoming  obstacle  as  well  as  to  look  for  the 
target  point  or  destination.  During  this  study  we  assumed  that  an  onboard  image 
processor  is  available  capable  of  detecting  multiple  obstacles  simultaneously  and 
distinguishing  them.  Since  the  measurements  from  a  vision  sensor  are  inaccurate,  it  is 
necessary  to  filter  the  noise  out  and  get  a  very  good  estimate  of  the  relative  position  of 
the  obstacle  before  the  guidance  can  be  applied.  The  filtering  technique  has  to  be  fast, 
computationally  efficient,  and  implemented  over  the  onboard  processor.  Since  the 
measurement  equation  from  a  vision  sensor  is  often  nonlinear,  an  Extended  Kalman  Filter 
(EKF)  is  a  very  suitable  technique  for  such  application.  It  is  recursive  in  nature  hence  can 
be  implemented  online  with  very  efficient  computing.  On  the  other  hand,  EKF  is 
“fragile”  is  nature,  so  care  must  be  taken  for  the  tuning  of  its  parameter  before  any 
application.  The  fundamental  assumption  with  this  filter  is  that  the  true  state  is  always 
sufficiently  close  to  the  estimated  state.  Hence  the  error  dynamics  can  be  represented 
fairly  accurately  by  the  linearized  system  about  the  estimated  state.  In  other  words,  if 
initial  estimation  error  is  significantly  high  then  it  is  very  difficult  to  converge. 
Nonetheless,  the  EKF  has  been  applied  successfully  for  a  range  of  problems  over  the  past 
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decades.  It  is  easy  to  implement,  computationally  efficient  and  works  quite  well  with 
most  of  the  problems.  Once  the  obstacle  position  is  estimated,  next  is  to  find  out  any 
potential  collision  with  an  obstacle  and  apply  some  fast  guidance  strategy  in  order  to 
avoid  it. 

Two  recently  developed  nonlinear  guidance  laws  [4],  named  as  Nonlinear  Geometric 
Guidance  (NGG)  and  Differential  Geometric  Guidance  (DGG),  are  implemented  here  for 
guidance  purpose.  This  navigation  laws  apply  collision  cone  approach  [3]  to  detect  any 
possible  collision  and,  if  necessary,  compute  an  alternate  aiming  point  in  order  to  avoid 
it.  The  guidance  algorithms  then  attempt  to  quickly  align  the  velocity  vector  of  the 
vehicle  along  the  aiming  point  within  a  part  of  the  available  time-to-go,  which  ensures 
quick  reaction  and  hence  safety  of  the  vehicle.  The  main  advantage  of  these  guidance 
laws  is  that  they  effect  high  maneuvering  at  the  beginning,  causing  the  velocity  vector  of 
the  UAV  to  align  with  the  aiming  point  direction  quickly  and  then  settle  along  it. 
Therefore,  there  is  no  need  to  maneuver  all  the  way  until  the  aiming  point  is  reached. 
Such  a  strategy  ensures  quick  reaction  and  hence  safety  of  the  vehicle  by  quickly 
avoiding  obstacle.  Note  that  after  the  obstacles  are  avoided,  the  destination  point  also 
serves  as  another  aiming  point  and  hence  the  same  guidance  is  also  applied  for  reaching 
the  destination  when  the  path  is  free  of  obstacles.  Therefore  these  guidance  laws  achieve 
reactive  obstacle  avoidance  as  well  as  destination-seeking. 

The  obstacle  position  estimation  technique  with  visual  information  developed  in  this 
paper  has  been  integrated  with  the  recently  developed  NGG  and  DGG  navigation  laws. 
This  paper  validates  these  guidance  strategies  with  vision  sensing  from  a  number  of 
simulation  studies  in  various  three-dimensional  scenarios. 


4 


Chapter  2 
Literature  Survey 

This  chapter  presents  a  literature  survey  on  recently  evolving  ideas  for  reactive  obstacle 
avoidance  for  UAVs.  Unlike  Global  Path  Planning  Algorithms,  reactive  obstacle 
avoidance  algorithms  deal  only  with  the  problem  of  avoiding  collisions  with  obstacles  as 
and  when  they  appear  during  the  flight.  These  algorithms  do  not  require  global 
information  i.e.  they  do  not  require  knowledge  of  the  entire  environment,  or  the  initial 
and  goal  points.  The  information  about  the  immediate  environment  and  nearby  obstacles 
is  provided  to  the  algorithm  by  onboard  sensors.  This  information  is  often  sufficient  to 
compute  an  avoidance  maneuver  for  the  UAV.  It  must  be  emphasized  that  these 
algorithms  can  be  imbedded  into  any  global  path  planning  algorithms,  under  the 
assumption  that  after  avoiding  the  obstacle,  the  UAV  comes  back  to  the  global  path  as 
soon  as  possible. 


2.1  Nonlinear  Model  Predictive  Control  Approach 


Model  Predictive  Control  (MPC)  [5]  has  gained  popularity  in  recent  years  as  a  control 
approach  for  nonlinear  dynamical  systems.  This  approach  handles  realistic  system 
constraints  such  as  input  saturation  and  state  constraints  and  is  found  to  be  suitable  for 
path-planning  problems  in  complex  environments  [6].  An  MPC  scheme  is  implemented 
by  [7]  to  achieve  online  obstacle  avoidance  in  UAVs.  Since  MPC  performs  online 
optimization  over  a  finite  receding  horizon,  it  can  account  for  future  environment 
changes.  Obstacle  avoidance  is  built  into  the  optimization  problem,  which  performs 
reference  trajectory  tracking  and  obstacle  avoidance  in  a  single  module.  When  a  collision 
is  predicted  to  occur,  a  safe  trajectory  that  avoids  the  impending  collision  is  computed 
(Figure  2.1).  In  the  MPC  formulation,  an  optimal  control  input  sequence  over  a  finite 
receding  horizon  N  must  be  found  that  minimizes  a  cost  function  [8]  that  is  to  find 

u(tk),k  =  i,i  +  \,i  +  N-l  (2.1) 

such  that 


u(tk  )  =  arg  min  V  (x,  tk ,  u) 


(2.2) 
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where 


lk+N-\ 

V(x,tk,u)=  X  L(x(i),u(i))  +  F(x(tk+N)) 


Original  Path 


Actual  Path  Followed 


Initial  Point 


Optimize  over  finite  horizon  N 


Figure  2.1:  Trajectory  tracking  and  re -planning  using  MPC 


Obstacle 


(2.3) 


L  is  a  positive  definite  cost  function  and  F  is  the  terminal  cost.  The  cost  function  L 
is  chosen  as: 

1  i  o 

L(x,u)  =  —  (xr  - x)T Q(xr  -x)  +  - urRur  +  S(x)  +  ^ P(xv ,11/)  (2.4) 

2  2  /= i 

The  first  term  in  the  cost  function  ensures  that  any  deviation  from  the  reference  state 
results  in  a  large  value  of  cost,  and  therefore  is  penalized.  Similarly  the  second  tenn 
penalizes  high  control  inputs.  The  term  S(x)  penalizes  states  that  are  outside  the  allowable 
range.  The  last  term  is  a  potential  function  term  to  be  included  for  obstacle  avoidance.  It 
is  chosen  as  follows: 


P(X 0,11/)  = 


1 


Ov-fi/)  G(xv-r |/)  +  s 


(2.5) 


where  xv  e  R  '  is  the  position  of  the  UAV,  and  p,  is  the  position  of  the  Ith  obstacle  out  of 


O  obstacles.  This  penalty  function  increases  as  ||xv-p,||  decreases.  G  is  positive 
definite  and  s  is  a  quantity  that  is  kept  positive  to  prevent  P  from  being  singular  when 
II  xv  -Th  ||— >  0 .  The  potential  function  term  may  be  chosen  to  be  enabled  only  if 

II  xv  _T1  /  ll<  ami„  5  a  minimum  safety  distance  to  be  maintained.  A  new  trajectory  is  then 
planned.  Including  collision  avoidance  into  the  optimization  step  reduces  the  risk  of  the 
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UAV  falling  into  a  local  minimum,  since  MPC  looks  ahead  and  optimizes  over  a  finite 
horizon. 

The  obstacle’s  predicted  position  after  k-N- 1  steps  is  required  (N  is  the  horizon) 
in  order  to  avoid  the  obstacle.  If  the  current  position  and  velocity  vl  of  the  obstacle  may 

be  estimated,  the  position  of  the  obstacle  after  N  steps  (prediction  horizon)  may  be 

found  at  the  k,h  step: 

Tl/(**+/)  =  rl/(0  +  Aty(4)0,-i)  (2-6) 

Control  saturation  is  taken  into  account  during  the  online  optimization  process. 
Additionally  a  tracking  feedback  controller  in  the  loop  will  track  the  reference  without 
any  error  in  the  presence  of  modeling  uncertainties. 

A  dual  mode  strategy  is  followed  in  order  to  track  a  reference  trajectory,  as  well  as 
avoid  collisions.  In  normal  flight,  parameters  are  chosen  so  as  to  achieve  tracking 
performance  and  good  stability.  In  the  emergency  evasive  maneuver  case,  the  parameters 
are  chosen  so  as  to  generate  a  trajectory  that  will  avoid  collision  at  all  cost.  During 
evasion,  large  control  effort  and  large  deviations  from  reference  are  allowed.  Results  of 
simulations  in  [8]  indicate  that  this  method  is  capable  of  avoiding  hostile  obstacles  flying 
at  high  speeds  (100  KM/H)  and  with  different  heading  angles.  This  method  was 
implemented  successfully  in  unmanned  helicopters.  A  disadvantage  of  this  method  is  that 
MPC  is  quite  computation  intensive  and  may  not  be  suitable  for  online  implementation 
on  small  UAVs.  An  extension  of  this  approach  can  be  towards  collision  avoidance  with 
maneuvering  obstacles.  This  would  require  an  estimator  and  some  knowledge  of  the 
dynamics  of  the  obstacle. 


2.2  Vision  Based  Approach 

Vision  based  navigation,  guidance  and  control  has  become  one  of  the  most  focused 
research  area  in  automation  of  robots  and  UAVs.  It  is  efficient  to  use  a  videos  sensor 
since  it  is  small,  light-weight,  economical  and  power  efficient.  Increasing  computational 
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power  of  small  processors  and  consequent  improvement  in  quality  of  digital  image 
analysis  is  another  motivation  for  using  vision  based  approach.  Additionally,  in  nature 
most  of  birds  and  insects  use  vision  for  collision  avoidance  and  navigation.  Following  are 
the  different  vision  based  algorithms  developed  to  address  the  problem  of  autonomous 
obstacle  avoidance  by  UAVs. 


2.2.1  Neural  Network  Approach 

A  vision  based  Grossberg  Neural  Network  (GNN)  [9]  scheme  is  used  for  local  collision 
avoidance  [10].  The  GNNs  are  nonlinear  competitive  neural  networks.  They  are  able  to 
explain  the  working  of  human  vision,  and  have  been  used  in  a  variety  of  vision  based 
applications,  especially  in  pattern  recognition  [11].  GNNs  can  be  used  for  UAV  vision 
based  navigation.  A  combination  of  Visibility  Graphs  and  GNNs  is  used  to  achieve 
online  collision  avoidance. 

A  two-layer,  dual-mode  control  architecture  achieves  a  formation  of  UAVs  as  well  as 
collision  avoidance.  The  top  layer  generates  a  reference  trajectory  and  the  lower  layer 
tracks  this  reference  taking  into  account  the  dynamics  of  the  vehicle.  In  an  obstacle-free 
environment,  “Safe  Mode"  operation  is  carried  out,  which  develops  and  maintains  a 
formation  of  UAVs.  When  obstacles  are  detected  using  an  on-board  sensor,  the  “Danger 
Mode"  is  activated,  which  finds  the  shortest  path  out  of  the  danger  zone.  In  the  “Safe 
Mode",  the  reference  trajectory  is  to  be  generated,  so  that  the  UAVs  achieve  and  maintain 
the  desired  formation.  The  relative  dynamics  between  the  UAVs  is  used  to  develop  an 
infinite  time  optimal  scheme  [12]  of  fonnation  in  a  centralized  way.  In  order  to  achieve 
this,  the  following  cost  function  is  to  be  minimized: 

oo 

J  =  J  [<X  -XdY  Q(x,  ~xd)  +  UrRUr  ]  dt  (2.7) 

t 

Subject  to 

xr  =  Arxr  +  Brur  (2.8) 

xr  is  the  relative  state  (relative  position  and  relative  velocity  between  two  UAVs)  and 
xd  is  the  desired  value  of  state.  ur  is  the  relative  control  i.e.  the  resultant  acceleration 


8 


between  two  UAVs.  This  formulation  attempts  to  attain  a  formation  as  well  as  minimize 
the  control  effort  for  this.  The  reference  trajectory  is  generated  online  at  every  step.  The 
danger  mode  is  activated  when  an  obstacle  is  sensed.  In  this  situation,  the  formation  is 
allowed  to  break.  The  UAVs  must  avoid  collisions  with  obstacles  as  well  as  with  the 
other  UAVs  in  the  formation.  The  danger  mode  operation  uses  a  combination  of 
Visibility  Graphs  [13]  and  vision  based  Grossberg  Neural  Networks  (GNN).  A  buffer 
zone  is  created  around  the  obstacles.  A  visibility  graph  of  the  environment  is  formed  by 
connecting  all  mutually  visible  vertices  of  the  obstacle  buffer  zones  together.  In  two 
dimensional  environments  the  shortest  distance  paths  are  obtained  by  moving  in  straight 
lines  and  turning  at  the  vertices  of  obstacles.  Therefore,  as  part  of  the  GNN,  neurons  are 
placed  at  the  vertices  of  each  obstacle's  buffer  zone.  Figure  2.2  shows  neuron  placement, 
visibility  graph  and  the  re -planned  trajectory. 


Initial  Point 


Figure  2.2:  Danger  Mode  operation  using  visibility  graph  and  GNNs 


The  activity  of  each  neuron  depends  upon  excitation  received  from  other  neurons  as 
well  as  excitation  from  the  goal  point.  The  activity  is  calculated  from  a  shunting 
equation: 


dx: 

dt 


k 


axi+(b-xi)(E  +  YJwijxj) 


(2.9) 


where 


E  =  Ex  +  E2 


(2.10) 


(2.11) 
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/ 100,  if  the  neuron  is  on  destination 
^  0,  otherwise 


(2.12) 


x(  is  the  activity  of  the  ith  neuron,  w.jc  .  is  excitation  due  to  neighboring  neuron,  where 


w..  =  ju/  dj  ,  dy  is  the  distance  between  UAVs  i  and  j  ,  dp  is  the  perpendicular  distance 

of  the  vertex  from  the  line  joining  the  UAV  and  the  target.  E  is  the  excitation  composed 
of  two  parts.  E]  is  the  excitation  due  to  closeness  of  the  vertex  from  the  present  path,  a 
and  //  are  weights  that  must  be  tuned  correctly  so  that  the  deviation  from  current  path 
and  closeness  to  goal  are  weighed  correctly.  The  neurons  nearest  to  the  goal  and  nearest 
to  the  current  path  have  the  highest  values  of  activity.  Thus,  by  following  the  vertices 
with  highest  activities,  the  UAV  is  able  to  get  out  of  the  danger  mode.  The  path  followed 
will  be  the  shortest  distance  path.  Collision  avoidance  with  other  UAVs  is  achieved  in  the 
following  way.  When  a  potential  collision  is  sensed,  the  UAV  with  lower  index  creates  a 
buffer  zone  around  the  UAV  with  higher  index  and  attempts  to  avoid  it. 


In  the  lower  layer,  tracking  the  reference  generated  by  both  the  Safe  Mode  and  the 
Danger  Mode  is  done  using  a  Model  Predictive  Controller  (MPC)  [14].  This  method 
consists  of  finding  the  optimal  control  input  sequence  to  minimize  a  cost  function  at 
every  step.  The  cost  function  here  is  formulated  such  that  the  actual  output  tracks  the 
reference  output,  along  with  control  minimization.  This  method  also  takes  into  account 
practical  vehicle  constraints.  The  cost  function  to  be  minimized  at  the  k'h  step  is  given  by 
following  equation: 

*4  =  [y(f )  -  X/  (h  )]r  Qk  [y(h )  -  yd  (fi )] + au  (c  )Rk  au  (tk )  (2.13) 

y  is  the  actual  output,  yd  is  the  desired  output  and  A U  is  the  control  history.  Qk  and 
Rk  are  weighting  matrices  to  be  chosen  appropriately.  Simulation  results  in  [10]  show 

that  the  UAVs  developed  a  desired  formation  and  successfully  re-planned  trajectories  to 
avoid  obstacles  along  the  way.  Cooperative  collision  avoidance  among  UAVs  is  also 
achieved.  A  possible  extension  of  this  method  is  the  case  of  non-cooperative  collision 
avoidance.  This  can  be  implemented  with  an  estimator  to  find  the  hostile  obstacles' 
velocity  and  position. 
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2.2.2  EKF  Estimation  Approach 


In  their  research  work,  [15]  focuses  on  vision-based  navigation  and  guidance  system 
design  for  UAVs  to  detect  and  avoid  unforeseen  obstacles  while  executing  a  waypoint 
tracking  mission.  Since  the  vision-based  measurements  are  noisy  and  inaccurate,  it  is 
necessary  to  filter  out  the  noise  before  applying  the  guidance.  Additionally,  visual 
measurements  are  also  a  nonlinear  function  of  the  relative  state.  Hence  EKF  is  a  very 
suitable  choice  to  design  the  navigation  filter.  The  UAV  motion  dynamics  are  modeled  as 
following  equations: 


X 

u 

V 

V 

A 

= 

Vv 

_K\ 

_WV_ 

u 

a 

V 

X 

V 

= 

a 

V 

y 

>vj 

X--_ 

=  V„ 


=  a 


(2.14) 


(2.15) 


here  ax  =0,  i.e.  velocity  is  constant  in  X-direction.  The  vehicle  is  controlled  by  only 


lateral  acceleration  a,  and  vertical  acceleration  a  . .  A  camera  is  mounted  on  the  vehicle 
and  its  attitude  is  assumed  to  be  known  in  the  form  of  a  rotation  matrix  from  the  local 
frame  FL  to  the  camera  frame  Fc  ,  which  is  denoted  by  LCI  .  Let  Xwp  =  [xwp  ywp  zwp]T 

be  a  given  waypoint  location  in  FL  .  Then  the  waypoint  tracking  problem  is  achieved  if 


yv(tf)  =  ywp,zv(tf)  =  zwp  (2.16) 

where  tf  is  a  time  at  which  xv(tf)  =  xwp  is  satisfied.  Let  Xohs  be  obstacle's  position  in  FL 
and  assume  Xobs  =  0 ,  i.e.,  stationary  obstacles.  Then  the  obstacle's  relative  motion 
dynamics  with  respect  to  the  vehicle  is  written  by 

X  =  Xglm-Xv=-Vv  (2.17) 

where  X  =  X obs  -  Xv  =  [x  y  z]r  is  a  relative  position  vector  in  FL.  For  collision 

avoidance,  the  vehicle  is  required  to  keep  a  minimum  distance  d  from  every  obstacle. 
That  is,  a  collision-safety  boundary  becomes  a  spherical  surface  with  a  radius  d  and  a 
center  at  Xobs .  Therefore,  a  mission  given  to  the  vehicle  is  to  satisfy  (2.16)  while  always 


maintaining  ||  X  ||  >  d  for  all  obstacles.  However,  the  obstacle's  location  Xohs  is  unknown 
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to  the  vehicle,  and  so  the  relative  position  X  is  also  unknown.  Hence,  for  obstacle 
avoidance,  the  guidance  system  can  only  use  its  estimate  which  is  obtained  through  a  2D 
vision  sensor. 


Figure  2.3:  Pin-Hole  Camera  Model 

The  camera  frame  Fc  is  taken  so  that  the  Xc-axis  aligns  with  the  camera's  optical 
axis.  Let  Xc  =  LaX  =  [xc  yc  zf]r  be  the  relative  position  vector  expressed  in  Fc  . 
Assuming  the  pin-hole  camera  model  shown  in  Figure  2.3,  the  2-D  measurement  of  the 
obstacle  position  in  an  image  plane  at  a  k'h  time  step  is  given  by 

+  ok=h{Xck)  +  ok  (2.18) 

where  /  is  a  focal  length  of  the  camera  and  vk  is  a  zero  mean  Gaussian  discrete  white 

noise  process  with  covariance  matrix  Rk  =  cr2 1 . 

Since  the  measurement  model  (2.18)  is  nonlinear  with  respect  to  the  relative  state,  an 
EKF  is  applied  to  estimate  the  relative  position  vector  X  of  each  obstacle.  The  EKF  for 
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the  process  model  (2.14),  (2.15)  and  the  measurement  model  (2.18)  is  fonnulated  as 
follows. 

•  Update:  The  EKF  update  procedure  is  perfonned  by  using  the  residual  between 
the  actual  measurement  and  the  predicted  measurement. 


Xk=Xk+Kk(zk-z-k) 

(2.19) 

Pk=P~-KkHkP- 

(2.20) 

Kk  =PkHTk(HkPkHTk  +Rky 

(2.21) 

where  Xk  is  an  updated  estimate  of  X  at  a  kth  time  step  and  Pk  is  its  error 


covariance  matrix.  Kk  is  a  Kalman  gain.  X k  and  Pk  are  a  predicted  estimate  and 
its  error  covariance  matrix.  A  predicted  measurement  is  obtained  by 
zk  =  h(Lc/kXk )  where  Lak  is  a  camera  attitude  at  the  k"1  time  step.  And  a 
measurement  matrix  II k  is  calculated  by 


Hk  = 


8KLCLkX) 


8X 


X  =  Xk  x-k 


.ZcL  1  0 


vck 


-3*  0  1 

Kk 


L  r 

CLk '  Kk  ^ 


-KX~ck)  I  Lak  (2.22) 


•  Prediction:  The  EKF  prediction  procedure  propagates  the  updated  estimate 
obtained  at  a  current  time  step  k  to  the  next  time  step  k  +  I  through  the  process 
model  (2.23),  (2.24). 


xk+ 1  ~  Kk+  VvkA tk  +  —  ak  (A tk ) 

P^=®kPk®Tk+Qk 


(2.23) 

(2.24) 


where  A tk  =  tk+l  -tk  is  a  sampling  time.  is  a  state  transition  matrix  and  which 
can  be  approximated  by 

dw 

for  stationary  obstacles  when  A tk  is  sufficiently  small,  Qk  is  a  covariance  matrix 
of  the  process  noise.  The  form  Qk  =  <j2xI  ■  A tk  is  used  in  the  fdter  design. 
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Since  there  can  be  multiple  obstacles  in  the  vehicle's  surroundings,  the  image 
processor  may  detect  more  than  one  obstacle  in  the  same  image  frame.  In  order  to  update 
each  estimate  correctly,  it  is  very  important  to  create  a  right  correspondence  between  the 
measurements  and  the  estimates  before  applying  the  EKF  procedure.  The  statistical  z-test 
[16]  is  used  for  this  purpose. 

The  problem  of  flying  a  UAV  with  vision  based  guidance  can  be  seen  as  a  control 
minimizing  Proportional  Navigation  (PN)  guidance  problem  in  the  absence  of  obstacles. 
PN  Guidance  is  used  in  missile  guidance,  [17]  and  the  problem  of  a  UAV  pursuing  its 
goal  may  be  interpreted  as  a  similar  problem.  But  when  obstacles  are  to  be  avoided, 
multiple  PN  guidance  problems  need  to  be  solved.  A  collision  avoidance  scheme  based 
on  PN  guidance  is  presented  in  [18].  However,  this  scheme  leads  to  a  jump  in  the  control 
effort  every  time  a  new  target  is  pursued.  Instead,  a  single  Minimum  Effort  Guidance 
(MEG)  approach  minimizes  the  control  effort  along  with  avoiding  collisions  for  multiple 
targets  [19].  In  other  words,  the  control  effort  is  minimized  for  the  entire  trajectory  from 
the  initial  point  to  the  goal  point  via  the  obstacle  aiming  point,  leading  to  a  lower  overall 
control  effort.  A  collision  cone  approach  is  used  to  detect  potential  collisions,  for  which 
an  Extended  Kalman  Filter  (EKF)  [20]  is  used  to  estimate  the  relative  distance  from  the 
UAV  to  the  obstacle. 


The  PN  guidance  law  is  derived  by  solving  the  following  optimization  problem  for 
control  minimization  for  each  aiming  point  obtained: 

■y  lgo  y  lgO 

min  Joa  =—  [  a  (t)a(t)  dt  =  —  [  (a2v(t)  +  ci2(t))  dt  (2.25) 

9  J  9  * 


Subject  to  vehicle  dynamics  with  terminal  conditions  given  in  (2.16),  the  resulting 
optimal  guidance  law  is 


«A(0  =  -3 


1 


0 

(t  _fv2\yM~yap 

{go  o)  1  z  (U)-z 

vV  0/  ap 


+  - 


1 


0 

v(t0) 


(2.26) 
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The  optimal  control  obtained  from  the  PN  Guidance  method  is  only  piecewise 
continuous,  with  a  jump  between  targets.  MEG  handles  all  the  tenninal  conditions  within 
one  problem  [21].  The  optimal  control  is  continuous  and  piecewise  linear.  This  method, 
therefore,  yields  a  lower  cost.  The  optimization  problem  remains  the  same  and  all  the 
tenninal  conditions  are  considered  in  the  problem.  The  control  law  is  found  by  cubic 
interpolation  of  the  single  condition  case.  The  resulting  optimal  control  law  is 


a{t0)  =  aoa{t)~ 


3(C,-0+4  (tf-tj 


v(t0) 

Mt0) 


(ho-h) 
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yM~yap 

zM-zaP 


(2.27) 
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In  [19],  both  PN  Guidance  and  Minimum  Effort  Guidance  are  used  to  solve  the  same 
problem.  The  cost  is  found  to  be  lower  in  MEG.  Therefore,  MEG  is  found  to  be  a  better 
method  in  terms  of  the  control  minimization  achieved.  However,  in  case  of  obstacle 
avoidance,  vehicle  safety  has  the  foremost  priority  so  minimum  control  effort  is  not  a 
requirement.  Also  MEG  maneuvers  the  UAV  till  it  reaches  the  aiming  point  which  is 
quite  risky  given  the  position  of  obstacle  is  not  known  with  full  certainty.  So  UAV 
velocity  vector  should  be  aligned  along  the  aiming  point  as  quick  as  possible  in  some  part 
of  time-to-go. 


o 


yap-yd 

-  z. 


2.2.3  Optic  Flow  Based  Approach 


The  obstacle  detection  through  optic  flow  is  an  already  widely  studied  problem  [22].  The 
objective  here  is  to  explain  changes  in  an  image  sequence  as  the  result  of  a  motion  field, 
given  an  image  sequence  It(x,y),  the  optical  flow  (vx,  v>:)  has  to  match  the  following 
optical  flow  equation: 


dl,  dl,  dl, 
—  v  +— vv  +  — 
dx  '  dy  -  dt 


0 


(2.28) 
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No  point  wise  resolution  of  the  optical  flow  equation  is  possible  since  on  each 
location  and  each  time,  this  would  consist  in  solving  a  single  scalar  equation  from  two 
scalar  unknowns.  This  is  known  as  the  aperture  problem.  [23]  and  [24]  survey  a  number 
of  ways  to  compute  optical  flow  of  a  changing  image.  The  most  appealing  algorithms 
tend  to  use  wavelets  to  interpolate  the  sparse  flow  data  in  between  the  borders. 

The  problem  of  collision  avoidance  has  been  addressed  by  the  use  of  optic  flow  in 
various  projects  [25].  There  is  a  relation  between  time  to  impact  and  optic  flow.  Optic 

Flow  during  motion  towards  an  object  is  a  direct  measure  for  time  to  impact  t  =  — .  In 

v, 

practice  this  temporal  distance  is  very  useful  to  trigger  a  well-dosed  reflex  to  avoid  a 
collision.  A  high  flow  meaning  high  risk,  this  high  optic  flow  can  be  used  as  trigger  for  a 
collision  avoidance  maneuver. 

2.3  Summary  of  Literature  Review 

Much  of  the  benefits  of  deploying  UAVs  can  be  derived  from  autonomous  missions.  Path 
planning  with  obstacle  avoidance  is  an  important  problem  which  needs  to  be  addressed  to 
ensure  safety  of  such  vehicles  in  autonomous  missions.  An  attempt  has  been  made  in  this 
paper  to  present  a  brief  overview  of  a  few  promising  and  evolving  ideas  such  as  model 
predictive  control,  vision  based  algorithms,  minimum  effort  guidance  etc.  As  mentioned 
earlier,  there  are  several  requirements  that  an  algorithm  must  satisfy  in  order  to  solve  the 
online  obstacle  avoidance  problem  completely.  A  few  key  issues  that  need  to  be 
addressed  in  a  good  collision  avoidance  algorithm  include: 

•  Collision  avoidance  with  fixed  obstacles,  as  they  appear  during  the  flight  without  any 
or  little  priori  information 

•  Solution  of  the  problem  taking  the  vehicle  dynamics  into  account,  including  state  and 
input  constraints  (many  of  the  current  algorithms  are  based  on  only  kinematics) 

•  Development  of  fast  algorithms,  which  can  be  implemented  online  with  limited 
onboard  processing  capacity 
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•  Capability  to  sense  and  avoid  small  obstacles  (such  as  electric  power  lines,  small 
birds  etc) 

•  Robustness  for  issues  such  as  limited  infonnation  of  the  environment,  partial  loss  of 
information  etc 

In  addition  to  the  above  issues,  there  are  many  other  issues  for  successful  deployment 
of  UAVs,  such  as  requirement  for  light  weight  equipments,  power  efficiency  (for  high 
endurance),  stealthness  etc.  Although  an  attempt  has  been  made  in  this  paper  to  give  an 
overview  of  some  of  the  recently  proposed  techniques  which  partially  address  some  of 
these  issues,  promising  algorithms  satisfying  many  of  these  requirements  simultaneously 
is  yet  to  be  developed.  Additionally,  some  of  the  assumptions  behind  the  proposed 
algorithms  (such  as  non-maneuvering  constant  speed  flying  objects,  appearance  of  one 
obstacle  at  a  time,  perfect  infonnation  about  the  environment  etc.)  are  not  realistic  and 
hence  need  to  be  relaxed.  A  lot  of  research  is  being  earned  out  worldwide  to  design 
collision  avoidance  systems  that  address  many  of  these  important  concerns. 
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Chapter  3 

Obstacle  Position  Estimation  Using  EKF 

In  this  section  we  describe  the  formulation  of  the  obstacle  position  estimation  problem. 
The  motivation  behind  using  vision  based  sensing  is  presented  first.  The  assumptions 
made  in  this  study  are  explained  next.  Then  the  problem  geometry  is  described.  Next 
section  provides  the  summary  of  continues-discrete  form  of  Extended  Kalman  Filter 
(EKF).  Section  3.3  explains  in  detail  how  EKF  is  applied  for  the  problem  of  obstacle 
position  estimation  with  vision  based  measurements. 

3.1  Vision  Based  Sensing 

As  stated  earlier,  certain  limitations  exist  for  any  system  or  algorithm  to  be  implemented 
onboard  a  small  flying  vehicle.  First  limitation  is  weight  constrain  i.e.  any  sensor  used  for 
obstacle  detection  must  be  light  weight  otherwise  it  might  seriously  compromise  the 
UAV’s  flying  and  maneuvering  capability.  Second  limitation  is  power  consumption. 
Generally  small  UAVs  are  battery  powered  and  they  do  not  have  much  extra  power 
available  onboard.  Power  consumed  by  obstacle  avoidance  system  will  also  limit  the 
UAV  flying  time  i.e.  limiting  the  UAVs  effectiveness  for  many  missions.  Another 
limitation,  which  concerns  the  military  missions,  is  stealthness  i.e.  UAV  should  not  be 
detectable  while  operative  inside  enemy  aerospace.  Thus  sensor  applied  must  be  passive 
in  nature  i.e.  while  sensing  the  surrounding  it  should  not  send  energy  signal  into  the 
environment. 

A  small  video  camera  addresses  all  these  concerns.  They  are  compact,  light-weight, 
power  efficient,  economical  as  well  as  passive  sensors.  These  advantages  make  a  video 
camera  very  popular  choice  in  such  application.  Increasing  computational  power  of  small 
processors  and  consequent  improvement  in  quality  of  digital  image  processing  is  another 
motivation  for  applying  vision  based  navigation.  Additionally  vision  based  navigation  is 
a  prevalent  phenomenon  in  nature  where  almost  all  kinds  of  birds  and  insects  exclusively 
use  vision  for  detection  and  navigation. 
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3.1.1  Assumptions  Made  in  this  Study 

Motivated  by  the  increasing  computer  power,  the  improving  quality  of  digital  imaging 
and  the  increasing  performance  of  digital  image  analysis,  this  algorithm  assume  that  an 
image  processor  is  available  which  is  capable  of  detecting  multiple  obstacles  and  target, 
simultaneously  from  the  images  obtained  from  2D  vision  sensor.  It  is  assumed  that 
obstacles  are  being  point  obstacles  with  known  safety  radius.  The  image  processor  can 
make  corresponds  between  the  present  measurements  of  obstacles  and  their  previous 
estimates.  It  is  also  capable  of  distinguishing  between  obstacles  and  target  point 
(destination). 


3.1.2  Problem  Geometry 

Figure  3.1  shows  the  problem  geometry.  For  simplicity  it  is  assumed  that  camera  is  fixed 
at  UAV’s  center  of  mass  and  the  UAV  knows  its  own  position  with  reasonable  certainty 
with  the  help  of  GPS  and/or  INS.  The  relationship  between  the  locus  of  obstacle’s 
projection  on  image  plan  and  its  relative  position  can  be  easily  shown  from  Figure  3.1 
using  symmetrical  triangles  as  given  by  (3. 1). 


y'k 

 / 

y  obk 

4_ 

Xobk 

_Zohk  _ 
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here  /  is  focal  length  of  the  camera,  Xr  =  [xob  yob  z  ]T  is  relative  position  of  the 

k  k  k 

obstacle  with  respect  to  the  UAV,  V=[u  v  w]ris  the  UAV  velocity  vector  and 


Yk  =  [y'k  z’ ]r  is  the  locus  of  the  obstacle  projection  on  the  image  plan  at  time  instant  k  . 
Objective  here  is  to  get  the  relative  position  of  obstacle  X  based  on  measurement  Yk  . 
Note  here,  equation  (3.1)  is  a  nonlinear  function  of  relative  state  Xr.  Additionally 

measurement  noise  is  also  present,  to  account  for  that  (3.1)  can  be  rewritten  as  following 
equation: 


r*  = 


yll 

_  f 

y  obk 

a\ 

Xobk 

_Zobk  _ 

+  V,, 


(3.2) 


here  vk  is  the  measurement  noise  at  time  instant  k .  Due  to  nonlinearity  and  noise,  it  is 
necessary  apply  some  filtering  technique  to  get  the  best  possible  estimate  of  X  from 
measurement  Yk .  Extended  Kalman  Filter  (EKF)  is  once  such  technique;  next  section 
provides  the  summary  of  the  EKF  in  the  context  of  our  application. 


3.2  Summary  of  Extended  Kalman  Filter 

A  large  class  of  estimation  problems  involves  nonlinear  models.  For  several  reasons,  state 
estimation  for  nonlinear  systems  is  considerably  more  difficult  and  admits  a  wider  variety 
of  solutions  than  the  linear  problem  [20],  In  a  vast  majority  of  nonlinear  models 
(including  ours),  system  states  are  continuously  evolving  and  measurements  are  available 
only  at  discrete  intervals  of  time.  Therefore,  continuous-discrete  fonn  of  EKF  best 
describes  our  system  model.  The  nonlinear  system  dynamics  and  measurement  model  can 
be  given  by  following  equations: 

X(t)  =  f(X(t),  U(t),  t )  +  G(t)w(t)  (3.3) 

Yk=h(Xk)  +  vk  (3.4) 

Where,  function  /  represents  the  system  dynamics  which  is  a  nonlinear  function  of  state 
X ,  the  deterministic  control  input  U ,  and  time  t .  The  process  noise  (or  disturbance 
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input)  w(t)  is  white,  zero  mean  Gaussian  random  process.  The  statistical  properties  of 
process  noise  w  can  be  written  as: 

fsTu^Ol  =  0 

r  t-  i  (3.5) 

E[w(t)w  (r)]  =  Q{f)8(t-v) 

The  function  h  represents  the  measurement  equation  which  is  a  nonlinear  function  of  the 
state  X .  The  measurement  noise  v  is  also  white,  zero  mean  Gaussian  random  process 
that  is  uncorrelated  with  process  noise.  The  statistical  properties  of  process  noise  v  can 
be  written  as  following  equations: 

£[v,]  =  o 

«[v,v/]  =  Vw  <16> 

£[»<(>/]  =  0 

The  basic  assumption  in  EKF  is  that  the  true  state  is  sufficiently  close  to  the  estimated 
state.  Therefore,  the  error  dynamics  can  be  represented  fairly  accurately  by  a  linearized 
first-order  Taylor  series  expansion.  So  we  linearize  the  nonlinear  system  around  the 
Kalman  filter  estimate  of  the  state.  The  linearized  matrices  are  given  as  following 
equations: 


dX 


X(t) 


r  -^L 

k  dX 


xk  (o 


(3.7) 

(3.8) 


The  expected  values  of  the  initial  state  and  its  covariance  are  assumed  known  and 
given  by  following  equations: 


X0=E{X0) 


(3.9) 


P0  = 


(3.10) 


A  weighting  factor  called  Kalman  filter  gain  computed  using  the  covariance 
information,  measurement  covariance  matrix  and  linearized  measurement  matrix  given 
by  (3.8).  The  Kalman  filter  gain  is  used  to  combine  the  propagated  estimate  with  the  new 
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measurement.  This  gain  is  defined  in  such  a  way  that  it  minimizes  the  estimation  error 
covariance  after  the  update.  The  Kalman  filter  gain  is  given  by  following  equation: 

Kk .=prc;[ctp,c; +«,]"'  (3ji) 


New  measurements  are  combined  with  the  propagated  state  estimate  to  generate  an 
updated  estimated  state.  The  state  covariance  is  also  updated  to  reflect  the  infonnation 
gained  through  the  measurement.  The  state  and  covariance  update  equations  are: 


x:=xk+Kk 


Yk-h(X~k) 


Pk+=(I-KkCk)Pk(I-KkCky+KkRkKk 


(3.12) 

(3.13) 


Between  measurements,  EKF  propagates  the  state  estimate  and  its  covariance  using 
the  known  nonlinear  dynamics  by  following  equations: 

h)  =  f(X(t),U(t),t)  (3.14) 

P(t)  =  A(t)P(t)  +  P{t)A(t)T  +  G(t)Q(t)GT  ( t )  (3.15) 

The  EKF  technique  requires  initialization  of  states  and  covariance,  computation  of 
Kalman  filter  gain,  update  of  states  and  covariance  followed  by  propagation  of  states  and 
covariance.  This  process  is  followed  in  a  cyclic  manner  except  for  the  initialization  part. 
Equation  from  (3.3)  to  (3.15)  constitutes  the  EKF  technique  for  continuous-time 
nonlinear  systems  with  discrete  measurements. 


3.3  Obstacle  Position  Estimation  using  EKF 

As  stated  earlier,  the  job  of  the  EKF  here  is  to  get  a  good  estimate  of  the  obstacle  position 
from  noisy  vision  measurements.  EKF  is  a  very  widely  used  technique  for  state 
estimation  in  nonlinear  systems.  This  section  provides  details  on  how  EKF  is  applied 
here. 


3.3.1  System  Dynamics  and  Measurement  Equation 

Initially  following  system  dynamics  are  considered  for  UAV  motion  modeling,  where  X 
is  ETAV’s  position  vector,  V  is  its  velocity  vector  and  a  is  the  control  vector. 
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x= y  =  v  =  V 


(3.16) 


V  =  v  =  \  a„  =  a 


(3.17) 


w  \  \  a. 


Furthermore,  the  velocity  along  X-axis  is  considered  to  be  constant  i.e.  ax  =  0 .  The 

UAV  is  modeled  as  a  point  mass,  so  video  sensor  (camera)  is  assumed  to  be  fixed  at  the 
UAV’s  center  of  gravity  and  its  orientation  with  respect  to  an  inertial  frame  of  reference 
is  known.  The  UAV  system  dynamics  given  by  (3.16)  &  (3.17)  is  linear  while  the 
measurement  equation  (3.1)  is  quite  nonlinear  function  of  the  state  vector  Xr .  It  is  not  a 

good  idea  to  have  a  nonlinear  measurement  equation  while  applying  EKF  [20],  since  its 
partial  differentiation  can  lead  to  quite  complicated  Jacobean.  Particularly  (3.1)  can  lead 
to  singularity  issue  as  UAV  approaches  close  to  the  obstacle  xob  — »  0  .  So  to  work  around 
these  issues,  it  is  better  to  shift  from  Cartesian  system  to  Spherical  coordinate  system  i.e. 
Xr  =  [rob  0oh  (f)ohJ .  Now  we  measure  the  locus  of  obstacle  projection  on  image  plane 
in  terms  of  angles  0[  and  <f>'k ,  instead  of  measuring  in  terms  of  y'k  and  z‘k  (Figure  3.2). 

So  measurement  becomes  a  linear  function  of  the  relative  obstacle  position  given  by 
following: 


Y=e‘ 

‘  U 


0  1  0 


Or  T 

1  \_r°bt  @°bk  & obk  ]  — 


0  1  0 
0  0  1 


(3.18) 


Since  state  vector  Xr  has  been  changed  from  Cartesian  to  Spherical  coordinate 


system,  the  system  dynamics  should  also  be  changed  accordingly. 
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UAV  Position 


The  well  known  relationship  between  Cartesian  and  Spherical  coordinates  is  given  by 
following  two  sets  of  equations  (3.19)  and  (3.20): 


2  2  2  2 
r  =x1+yi+z1 


tan  0  =  — 


(3.19a) 

(3.19b) 


tan^  = 


and 


x  =  r  cos  0  cost/) 
y  =  rsin^cos^ 
z  =  r  sin  (j> 


(3.19c) 
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(3.20a) 

(3.20b) 

(3.20c) 


differentiating  (3.19a) 


since  x  =  u  ,  y  =  v ,  and  z  =  w 


2rr  =  2xi  +  2  yy  +  2zz 


rr  =  xu  +yv  +  zw 


after  substituting  (3.20a),  (3.20b)  and  (3.20c) 


rr  =  ur  cos  #  cos  (j)  +  vr  sin  #  cos  (j)  +  wr  sin  (j) 
r  =  u  cos#cos^  +  vsin#cos^  +  wsin^ 


Similarly  differentiating  (3.19b) 


sec^  9  6  =  — ^-x  +— y 

X '  X 

sec2  69  =  — 4«  +  -v 


(3.21) 


after  substituting  (3.20a),  (3.20b)  and  (3.20c) 


sec2  99  =  -- 


r  sin  9  cos  (j) 


-u  +  - 


1 


(r  cos#  cos  0)"  r  cos#  cos  ^ 

a  sin#  cos# 

#  = - w  + - V 

r  cos  (j)  r  cos  (j) 


Finally  differentiating  (3.19c) 


2/i  xz  .  yz  1 

sec ~##  = - a - mrx - , - txjt  v  +  <  z 

u  +r>  u2+r),,j  yyyy 

2,1  XZ  VZ  1 

sec  00  = - , - - O - T7TV  +  — j  w 

(*J+rf!  (rtv’f  ,/?77 

after  substituting  (3.20a),  (3.20b)  and  (3.20c) 

;  cos  #  sin  #  sin#  sin#  cos# 

(p  = - u - v-\ - w 

r  r  r 


(3.22) 


(3.23) 


By  combining  (3.21),  (3.22)  &  (3.23),  the  system  dynamics  become  a  nonlinear 
function  of  the  state  vector  given  by  following: 
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X,.  = 


'ob 


e 


ob 


<P, 


ob . 


it  cos  0ob  cos  (f)oh  +  v  sin  0ob  cos  (f)oh  +  vvsin  (f>i 


ob 


sin# 


ob 


U+- 


cos  0, 


ob 


robCOS<f>ob  robCOS^c 


^ ob 


cos  6oh  sin  <f>ob  sin  6ob  sin  </>ob  cos  </>ob  _ 

- U - V  H - W 


'ob 


'ob 


'ob 


(3.24) 


3.3.2  Measurement  Noise  Model 

Usually  the  measurement  data  from  vision  sensor  is  inaccurate  and  noisy.  It  is  assumed 
that  measurement  noise  is  zero  mean  Gaussian  process.  Another  assumption  is  that  the 
magnitude  of  the  measurement  noise  is  a  function  of  object  range  i.e.  higher  the  distance 
between  the  sensor  and  the  object,  higher  will  be  the  measurement  uncertainty.  It  is  a 
reasonable  assumption  since  it  is  common  knowledge  that  closer  you  get  to  an  object, 
higher  will  be  the  quality  of  the  visual  information  obtained.  Based  on  this  philosophy,  a 
function  of  range  has  been  devised  to  calculate  the  measurement  noise  covariance.  First 
we  calculate  the  maximum  amount  of  noise  that  can  be  added  by  following  function. 

rk=r0(l-Sr°b>)  (3.25) 

here  rk  is  the  maximum  percentage  noise  at  time  instant  k ,  r0  is  a  constant  which 
represents  the  highest  possible  amount  of  percentage  measurement  noise  (as  rob  — >  oo  ), 
r  h  is  the  range  of  the  object  at  time  instant  k  ,  and  8  is  a  parameter  (close  to  1)  which 
defines  how  rk  changes  with  change  in  rob  .  Again  based  on  common  knowledge  on 
visual  sensing,  it  is  assumed  that  rk  changes  slowly  for  higher  range  value  and  drops 
quickly  for  lower  values  of  range.  Figure  3.3  shows  variation  in  rk  with  range  from  zero 
to  1000  meters  for  r0  =  20  and  S  =  0.99  . 
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Figure  3.3:  Measurement  Noise  as  a  function  of  Object  Range 
After  calculating  the  maximum  percentage  noise  at  time  instant  k  by  (3.25),  we 
calculate  the  measurement  noise  covariance  by  following  equation: 


Rk  = 


(  wv  n 


-X  — 


100  3 


(3.26) 


here  Wv  represents  the  angular  width  of  the  camera’s  field  of  view.  Here  it  is  assumed 


120°  on  both  horizontal  and  vertical  axis.  The  whole  expression  inside  the  bracket 
represents  the  standard  deviation  of  the  measurement  noise.  Since  noise  is  normally 
distributed,  above  equation  insures  that  measurement  noise  will  be  bounded  by  rk  or  3cr 
for  almost  all  of  the  cases  (more  than  99%). 


3.3.3  Process  Noise  Model 

Any  real  system  whether  linear  or  nonlinear  has  some  amount  of  process  noise.  Process 
noise  arise  from  parameters  which  are  not  modeled  in  system  dynamics  either  because 
their  very  small  effect  or  because  of  their  mathematical  intractability.  For  example,  in  our 
model,  the  effects  of  atmosphere  such  as  random  wind  gusts,  variation  of  gravity  with 
altitude  etc  are  ignored.  However,  they  do  have  some  effect  over  the  system  dynamics 
and  hence  it  is  necessary  to  accommodate  them  in  our  system  model.  The  process  noise  is 
modeled  by  adding  fictitious  noise  w(t)  in  system  dynamics.  The  process  noise  is 
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considered  as  Gaussian  random  input.  The  system  dynamics  given  by  (3.24)  can  be 
rewritten  to  account  for  process  noise  as  following: 


X,.  = 


u  cos  0ob  cos  (()oh  +  vsin  9ob  cos <f>oh  +  vvsin  </>c 


t'ob 


sin  9, 


ob 


-u  +  - 


cos  6. 


ob 


robCOS<f>ob  robCOS</>c 


f'ob 


cos  eob  sin  (j)ob  _  sin  6oh  sin  ^  cos  <f>ob 

- U - V  H - W 


'ob 


'ob 


'ob 


+  G(t)w{t ) 


(3.27) 


here  G(t )  is  the  process  noise  realization  matrix  and  w(t)  is  zero  mean  Gaussian  noise 
with  covariance  given  by  (3.5).  It  is  a  reasonable  assumption  that  process  noise 
covariance  Q{t )  can  be  considered  constant  for  a  given  system  over  a  short  period  of 
time,  hence  can  be  denoted  as  just  Q .  Additionally  process  noise  in  each  state  element  is 
considered  to  be  independent  of  each  other  hence  process  noise  realization  matrix  will  be 
unity  i.e.  G(t)  =  /  . 


3.3.4  Initialization  of  the  EKF 

Initialization  of  the  EKF  requires  initial  value  of  the  state  vector  and  initial  error 
covariance  matrix.  However,  knowing  these  two  quantities  at  start  is  not  possible  in  most 
of  the  real  systems.  That’s  why  it  requires  some  amount  of  priori  information  or  some 
empirical  knowledge  of  the  system  to  make  an  educated  guess. 

As  stated  earlier,  assuming  that  UAV  is  equipped  with  an  image  processor  capable  of 
identifying  all  the  obstacles  present  in  its  view  field,  EKF  is  initialized  based  on  the  first 
processed  image.  The  number  of  obstacles  in  the  first  image  defines  the  length  of  state 
vector.  For  example,  if  image  contains  two  obstacles  and  target  (or  waypoint),  then  the 
state  vector  will  consist  of  9  elements,  where  each  3  elements  will  represent  the  relative 
position  of  one  of  the  three  objects  (whose  position  needs  to  be  estimated).  Similarly  in 
case  of  one  obstacle  and  target,  state  vector  will  contain  only  6  elements,  as  shown  in 
following  equations  (3.28)  and  (3.29)  respectively: 

E-(0)  =  [g1(0)  <9jo)  <Uo)  rob2  (O)  iJo)  ^  (0)  r  (o)  0r  (())  <i(0)f(3.28) 
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(3.29) 


^,(0)  =  [U°)  4(o)  L{ 0)  ^(0)  4(0)  ^.(0)]r 

Since  obstacles  are  assumed  to  be  point  obstacles,  it  is  not  possible  for  an  obstacle  to 
appear  from  behind  another  obstacle.  So  there  is  no  need  to  augment  the  state  vector  to 
accommodate  newly  discovered  obstacle  while  EKF  is  running.  However,  it  is  not  a 
realistic  assumption  and  needs  to  be  relaxed  in  future  studies. 

•  Initialization  of  State  Vector:  For  simplicity  of  discussion,  we  will  keep  state  vector 
3  elements  long  i.e.  we  will  estimate  the  position  of  one  object  only.  State  vector  can 
be  augmented  in  case  of  simultaneously  estimating  the  multiple  objects.  The  first 
image  contain  the  9  and  <p  information  (with  measurement  noise)  of  the  object  needs 
to  be  estimated.  However,  2D  vision  sensing  does  not  contain  any  infonnation  about 
the  range  or  r  .  For  initialization  purpose,  range  of  the  object  is  assumed  known  with 
50%  uncertainty.  Following  equation  shows  initialization  of  state  vector. 

-f(0)  =  [U°)  <U°)  (U°)I  (3.30) 

here  rob( 0)  is  known  as  50%  error  while  9ob{ 0)  and  0oA(O)  are  based  on  first 

measurement  taken  and  initial  measurement  noise  given  by  (3.25).  Initial  estimation 
of  an  object  requires  some  amount  of  priori  infonnation  (in  the  form  of  their  range 
information).  The  6  and  (f)  estimates  will  be  better  for  the  closer  objects  (because  of 
lower  measurement  noise)  which  is  consistent  with  the  fact  that  we  will  have  better 
visual  for  the  objects  which  are  closer  to  us. 

•  Initialization  of  Error  Covariance  Matrix:  Theoretically,  the  error  covariance 
matrix  is  given  by  following: 

P0  =F[(Xr(0)-X;.(0))(X;.(0)-X;.(0))r]  (3.10) 

here  Xr(0)  is  the  actual  initial  value  of  the  state  vector  while  Xr(0)  is  the  initial 
estimation  of  the  state  vector.  However,  Xr(0)  is  not  known  so  the  initialization  of 
error  covariance  matrix  requires  some  idea  of  uncertainty  in  initially  estimated  state. 
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P0  should  be  sufficiently  high  enough  to  contain  maximum  amount  of  error  in  the 
initial  state  estimation.  Based  on  known  initial  uncertainty  in  range  estimation  and 
initial  measurement  noise,  P0  is  initialized  as  the  following  diagonal  matrix: 


here  e  is  the  maximum  range  measurement  uncertainty  (in  meters)  given  by  the  50% 
of  the  distance  between  the  start  point  and  the  destination,  r0  is  maximum  percentage 

measurement  noise.  al ,  a2  and  a3  are  scalar  parameters,  which  needs  to  be  tuned 
based  on  trial  and  error.  Wv  is  camera’s  width  of  view  (in  radians).  Initially  it  is 

assumed  that  estimation  errors  are  independent  of  each  other  i.e.  error  in  one  state 
element  does  not  affect  the  others  and  so  on.  Note  that  the  dimensions  of  P  matrix 
will  depend  on  the  length  of  the  state  vector.  For  example  in  case  of  a  state  vector 
with  six  elements,  P0  matrix  will  be  a  6x6  diagonal  square  matrix. 


3.3.5  Pre-Run  of  EKF 

While  applying  EKF,  it  is  highly  recommended  that  filter  run  sufficiently  ahead  of  time 
so  that  initial  error  can  be  stabilized  before  its  actual  application.  Since  our  system  is  a 
closed  loop  system,  if  error  in  initial  estimation  is  high,  the  corresponding  control  can 
completely  destabilize  the  whole  system.  Hence  we  start  EKF  10  seconds  before  applying 
any  control  i.e.  UAV  fly  towards  its  initial  velocity  vector  and  EKF  just  estimate  the 
relative  position  of  the  obstacles  and  target  without  applying  any  guidance.  In  other 
words,  system  behaves  like  an  open  loop  system  during  the  pre-run  of  EKF.  After  that,  it 

reinitializes  the  X r ( 0)  as  the  average  of  all  previous  estimates  as  given  by  (3.32)  and  P0 
again  as  (3.31). 

*r(°)=^Z*r(0  0-32) 

A  f=i 

here  N  is  the  number  of  estimations  made  by  EKF  during  pre-run.  After  reinitializing 
the  state  vector  and  error  covariance  matrix,  the  system  transforms  into  a  closed  loop 
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system  and  starts  applying  the  control  based  on  estimated  relative  position  of  the 
obstacle/s  and  target. 


3.3.6  Propagation  of  State  and  Error  Covariance 

The  operation  of  EKF  can  be  divided  into  two  phases,  Propagation  and  Updation.  In 
propagation  phase,  EKF  predicts  the  next  state  based  on  previous  state  and  known  system 
dynamics  i.e.  propagation  of  state  from  posterior  estimate  at  time  instant  k  to  a  priori 

estimate  at  k  + 1  or  mathematically  V.  — >  V,  |  .  Based  on  the  system  dynamics  derived 
in  (3.24),  the  state  propagation  equation  of  our  system  is  given  by  following: 


u  cos  0ob  cos  (f)oh  +  v  sin  doh  cos  <j>oh  +  w  sin  <fioh 
sin  0.  cos#, 

- U  + - 

ibCOS<t>ob  pot cos 


cos  eob  sin  f bob  _  sin  0oh  sin  <t>ob  cos  </>ob  _ 

- U - V  i - W 


rob 


'  ob 


'  ob 


(3.33) 


here  V  =  \u  v  vv] 7  is  present  relative  velocity  vector  between  the  EIAV  and  the  object 
being  estimated.  Since  in  this  study  only  stationary  obstacles  are  considered,  vector 
V  =  -VUAV  i.e.  negative  of  UAV  velocity  vector  VUAV  and  Xr=\roh  0oh  (f>oh]'  is 
previous  estimate  of  the  relative  position  of  the  object  being  estimated. 


After  propagating  the  state  vector,  we  propagate  the  error  covariance  matrix  or  the  P 
matrix.  The  P  matrix  propagation  equation  is  given  in  section  3.2  EKF  summary. 

P(t)  =  A(t)P(t)  +  P(t)AT  ( t )  +  G(t)Q(t)GT  (0  (3.15) 

Since  G(t)  =  I  (from  section  3.3.3)  and  Q(t )  is  constant  (can  be  denoted  as  just  Q ), 
above  equation  can  be  modified  as  following  equation: 

P(t)  =  A(t)P(t)  +  P(t)AT(t)  +  Q  (3.34) 


here  A(t)  = 


i.e.  partial  differential  of  the  nonlinear  state  equation  with  respect 


to  the  state  vector  evaluated  at  a  priori  estimate  of  the  state  vector.  The  expression  for  the 
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matrix  A(t )  is  derived  next  in  this  section.  After  partially  differentiating  X r  (System 


dynamics  given  by  (3.24))  with  respect  to  state  vector  Xr,  the  matrix  A  is  given  by 
following  expression. 


0 


A  = 


u  sin  9  -  v  cos  9 
r~  cos  (f> 

u  cos  9  sin  $  +  v  sin  9  sin  $ 

2 

r 


WCOSfZi 


cos  (f>(-u  sin  9  +  v  cos  9) 
u  cos  9  +  v  sin  9 
-r  cos  (j) 

sin  <j>(u  sin  9  -  vcos  9) 
r 


-u  cos  9  sin  (f>  -  vsin  <9  sin  (f>  +  wcos  ([> 
sin  i/>(-u  sin  9  +  v  cos  9) 
r  cos"  (f> 

-u  cos  9 cos (f>  -  v  sin  9  cos  (j>  -  wsin  cj) 
r 

(3.35) 


3.3.7  Updation  of  State  and  Error  Covariance 


In  Updation  phase  of  operation,  EKF  updates  or  corrects  the  previously  predicted  state 
and  error  covariance  based  on  the  newly  arrived  measurements.  After  getting  the 
measurements  according  to  the  following  equation 


E  = 


0  1  0 
0  0  1 


X„  +v. 


(3.36) 


here  vk  is  the  zero  mean  Gaussian  noise  with  covariance  given  by  (3.14).  Next  step 
before  Updation  is  to  compute  the  Kalman  Gain  by  following: 

Kk  =  PkCl[ctP^CTk+RtY  (3.37) 

here 


"  dh " 

"0  1  0" 

dXr 

K 

0  0  1 

(3.38) 


and  Rk  is  measurement  noise  covariance  given  by  measurement  noise  model  described  in 


section  3.3.2.  After  calculating  the  Kalman  gain,  next  is  update  the  state  estimate  and 
error  covariance  matrix  based  on  following  equations: 


x:  =  x:  +K,, 


L k 

K  =  (/  -  KkCk)p-(I  -  KkCky  +  KkRkKTk 


Yk-h(X;) 


(3.39) 

(3.40) 


As  seen  above,  the  Kalman  gain  is  a  very  important  parameter  in  the  EKF  Updation 
phase.  Kalman  gain  defines  where  to  pay  more  attention  while  updating  the  state  estimate 
between  measurements  and  system  dynamics.  From  above  equations  it  is  clear  that  higher 


32 


the  Kalman  gain,  higher  will  be  the  effect  of  the  measurements  over  the  state  Updation 
and  vice  versa. 


3.3.8  Smoothing  of  Estimate 

Due  to  measurement  uncertainties,  sometimes  due  to  momentarily  high  noise  position 
estimates  tend  to  fluctuate  i.e.  newly  estimated  object  position  differs  too  much  from 
previous  estimates.  Since  it  is  a  closed  loop  system,  these  fluctuations  can  produce  large 
associative  control  accelerations  and  which  can  severely  destabilize  the  whole  system. 


To  avoid  that,  it  is  better  to  smooth  the  new  estimate  with  respect  to  the  previous 
estimates  i.e.  instead  of  using  the  current  estimate  only  for  guidance  purpose,  first  take 
the  average  of  current  estimate  with  few  previous  estimates  and  then  apply  the  guidance 
according  to  the  averaged  or  “smoothed”  estimate.  Deciding  how  many  previous 
estimates  used  for  smoothing  operation  should  be  done  carefully.  It  is  a  tradeoff  between 
trusting  a  newly  arrived  estimate  and  trusting  previous  estimates.  If  we  use  too  many 
previous  estimates,  the  effect  of  any  new  estimate  will  almost  zero.  Similarly,  too  few 
previous  estimates  will  not  affect  a  large  estimation  fluctuation  at  all.  Since  we  are  using 
a  range  dependent  measurement  noise  model,  the  measurements  are  improving  as  UAV  is 
getting  closer  to  the  object.  Thus  more  emphasis  should  be  given  to  the  newly  arrived 
estimates.  Based  on  this  philosophy,  after  some  trial  runs  it  is  found  out  that  algorithm 
works  best  while  smoothing  operation  is  performed  with  ten  previous  estimates.  The 
smoothing  operation  performed  by  following  equation.  Note  that  smoothing  is  only 
required  after  the  pre-run  of  EKF  is  over  and  system  is  working  as  a  closed  loop  system. 


1  k 

T  1  I  at. 


n  i=k-n- 1 


(3.41) 


here  n  is  the  number  of  previous  estimates  used  for  smoothing  operation  (n  =  10  in  our 
case),  X  is  smoothed  estimate  and  Xt.  original  estimate  at  time  instant  k  . 
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3.3.9  Tuning  of  EKF 

After  developing  the  whole  EKF,  its  tuning  is  the  final  step.  Tuning  of  EKF  requires 
proper  selection  of  parameters  Q ,  P0  and  R,  .  As  stated  earlier,  EKF  is  fragile  in  nature 

i.e.  it  works  well  only  for  a  narrow  band  of  Q ,  P0  and  R  parameters.  Hence  tuning  of 
EKF  should  be  done  carefully. 


Since  we  are  using  a  range  dependent  measurement  noise  model,  parameter  Rk  is  fixed 

by  measurement  noise  model  given  by  (3.26).  In  case  of  one  obstacle  and  target 
estimation,  Rk  is  given  by  following  formula: 


Rf  =  diag 


(  wv  if  (  wv  n2  (  wv  iV  (  wv  n 
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100  X  3 


(3.42) 


here  Wr  is  the  angular  width  of  the  camera’s  view  and  assumed  to  be  —  ;r  radians  ( 120°) 


for  both  vertical  and  horizontal  axis.  robt  and  r  are  the  maximum  percentage  noise  in 

measurement  of  angles  for  obstacle  and  target  respectively.  They  are  calculated  by  (3.25) 
given  in  section  3.3.2  based  on  range  dependent  measurement  noise  model. 


P(0)  is  selected  by  some  prior  information  about  error  in  initial  estimation  of  state.  This 
knowledge  can  be  empirical  or  can  be  an  educated  guess.  It  is  given  by  following 
equation  (3.43)  for  simultaneous  estimation  of  an  obstacle  with  target: 


P0  =  diag 


^\^ob  ^2 


rob0XWV 

100 


rob0xWr 
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100 


(\xW^ 

100 


2  A 


(3.43) 

here  eob  and  etr  are  the  maximum  range  estimation  uncertainty  for  obstacle  and  target 
respectively  (in  meters  given  by  the  50%  of  the  distance  between  the  start  point  and  the 
destination)  and  assumed  known.  ax,  a2,  a3,  a4,  a5  and  a6  are  scalar  parameters,  which 
are  tuned  to  following  values  with  trial  and  error  method. 
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(3.44) 


<2j  =  1  a4=  1 

a2=  2  a5  =  2 
a2  =  2  a6  =  2 

The  process  noise  covariance  (9  set  to: 

Q  =  diag(  0.2  0.025  0.025  0.2  0.025  0.025)  (3.45) 

here  the  diagonal  elements  of  Q  matrix  are  selected  through  trial  and  error  method.  First 
and  fourth  diagonal  elements  of  Q  matrix  represent  the  process  noise  covariance  (in 
meter)  for  the  range  elements  of  the  state  vector  shown  in  equation  (3.29).  Similarly 
second,  third,  fifth  and  last  diagonal  elements  represent  the  process  noise  covariance  for 
angle  elements  (in  radians)  of  state  vector. 
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Chapter  4 

Guidance  and  Navigation 

Once  the  obstacle  position  is  estimated,  the  objective  reduces  to  applying  the  guidance  to 
navigate  the  UAV  around  it.  Now  first  task  is  to  finding  out  whether  obstacle  is  critical 
i.e.  if  collision  with  obstacle  is  imminent.  For  that  Collision  Cone  approach  is  applied. 
This  technique  detects  collisions  and  computes  an  alternate  aiming  point  (if  necessary). 
Then  the  geometry  of  the  resulting  guidance  problem  is  analyzed.  These  steps  aid  in 
forming  the  guidance  objective.  Finally  two  nonlinear  guidance  laws  used  to  achieve  the 
guidance  objective,  Nonlinear  Geometric  Guidance  (NGG)  and  Differential  Geometric 
Guidance  (DGG)  are  explained. 

4.1  The  Collision  Detection  and  Aiming  Point  Computation 

The  UAV  must  detect  an  imminent  collision  and  avoid  it  safely.  The  “collision  cone"  [3] 
is  an  effective  tool  for: 

(i)  Detecting  collision 

(ii)  Finding  an  alternate  direction  of  motion  that  will  avoid  the  collision 

In  this  approach,  a  collision  cone  is  constructed  and  analyzed  for  every  obstacle.  The 
collision  cone  approach  is  used  to  find  a  safe  aiming  point  Xap  and  the  time-to-go  to  the 

aiming  point  tpo .  A  suitable  guidance  law  should  then  be  used  to  steer  the  UAV  to  an 
aiming  point  Xap  in  time  tpo .  The  construction  of  the  collision  cone  is  shown  in  Figure 

4.1.  A  spherical  safety  boundary  of  radius  d  is  constructed  around  the  obstacle.  An 
obstacle  is  considered  to  be  critical  if  the  UAV  is  expected  to  violate  the  safety  boundary 
in  future.  Xr  is  the  relative  distance  between  the  UAV  and  the  obstacle  and  V  is  the  total 
velocity.  Since  the  collision  cone  approach  operates  in  two  dimensions,  the  plane 
containing  X r  and  V  is  considered  for  constructing  the  cone.  The  safety  boundary  thus 
reduces  to  a  circle  ft  in  this  plane.  A  collision  cone  is  constructed  by  dropping  tangents 
from  the  UAV  to  the  circle  fi .  If  the  velocity  vector  V  lies  within  the  collision  cone,  the 
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UAV  will  violate  fd  in  due  course  and  result  in  collision.  Thus  the  obstacle  is  said  to  be 


critical.  From  Figure  4.1,  V  can  be  expressed  in  terms  of  the  tangents  f  and  T2  as 
follows: 

V  =  ar1+br2  (4.1) 


Aiming  Point  X ( 


^Goal 

yd  ZA 


The  collision  criterion  is  stated  as: 

If  a  >  0  AND  b  >  0 ,  the  obstacle  under  consideration  is  said  to  be  critical 
The  aiming  point  is  then  found  from  the  collision  cone.  First,  the  tangents  r\  and  r2 
are  found  as  follows: 


rx  =Xr+  dux 
r2  =  Xr  +  du2 


(4.2) 


where  w,  and  u2  are  the  unit  vectors  perpendicular  to  the  tangents.  The  aiming  point  is 
determined  in  the  following  way: 


if  a  >b,Xap  =Xv  +  rx 
f  b>  a,  Xap  =  Xv  +  r2 


(4.3) 


Since  the  velocity  in  x  direction  is  assumed  constant,  the  time-to-go  t  is  found  as 
follows: 
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(4.4) 


u 

The  expressions  for  a ,  b  and  Xap  are  derived  in  Appendix  A.  Two  practical  issues 

are  expected  to  arise  in  the  implementation  of  the  collision  cone  approach.  We  list  these 
issues  and  describe  the  solution  methodology  for  each. 

•  Obstacle  safety  bound  violation  after  aiming  point  is  reached:  An  obstacle  may  be 
considered  critical  only  so  long  as  the  vehicle  is  behind  it  (i.e.,  the  x-coordinate  of  the 
vehicle  is  less  than  the  x-coordinate  of  the  obstacle).  Once  the  UAV  is  past  an  aiming 
point,  it  immediately  looks  to  maneuver  towards  the  next  aiming  point.  This  may 
result  in  a  brief  violation  of  the  first  obstacle's  safety  bound  if  the  direction  of  the  new 
aiming  point  lies  in  the  opposite  side  of  the  safety  sphere.  Such  a  scenario  is 
illustrated  in  Figure  4.2.  In  order  to  remedy  this,  a  sphere-tracking  algorithm  is 
activated  when  X r  <  d  .  The  sphere  tracking  algorithm  computes  a  new  aiming  point, 
called  the  virtual  aiming  point  which  is  a  point  on  the  surface  of  the  safety  sphere. 
This  is  found  by  radially  extending  the  original  relative  distance  line  Xr  until  it 
meets  the  surface  of  the  safety  sphere.  The  UAV  then  aims  for  the  virtual  aiming 
point  until  Xr  >  d  .  The  mathematical  details  of  the  sphere  tracking  algorithm  may  be 
found  in  Appendix  B. 

•  Intersecting  obstacle  safety  boundaries:  Figure  4.3  illustrates  the  problem  of  safety 
boundary  intersection.  It  is  apparent  from  the  vehicle's  orientation  that  obstacle- 1  is 
critical,  and  that  the  UAV  must  aim  towards  X p2  ( I  ,  and  Xp7  are  the  two  choices 

for  the  aiming  point  calculated  from  the  collision  cone  approach).  However  X p7  is 

clearly  illegal  as  it  lies  within  the  safety  circle  of  obstacle-2.  The  algorithm  must 
therefore  be  able  to  identify  this  issue  and  maneuver  the  UAV  to  X pX  .  We  solve  this 

by  determining  the  center  P2  of  the  intersecting  area  of  the  two  circles  [26], 

f  p  ~p  \ 

P2  =  P0  +  m  -X—H  (4.5) 

v  l 
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/  2,2  2 

/  +  Y  —  Y 

where  /  =||  Pl  —P0 1|  and  m  = - ^ — —  .  We  then  choose  the  aiming  point  that  is 

farther  away  from  Pi,  i.e., 

z/  II X pX  —P2 1|  >  ||  X p2  —P2  ||,  then  Xap  =  Xpl 
if\\Xpl-P2\\<\\Xp2-P2\\,then  Xap=Xp2  {"X" 


Figure  4.2:  Safety  boundary  violation  after  aiming  point  is  reached 

The  collision  cone  approach  constitutes  an  effective  tool  for  detecting  impending 
collisions  and  finding  an  aiming  point.  The  collision  avoidance  problem  then  becomes 
one  of  guiding  the  UAV  from  Xv(t0)  =  Xin  to  Xv(t0  +tg0)  =  Xap .  Note  that  when  no 

obstacles  are  critical  Xap  =  X d .  The  collision  avoidance  problem  therefore  becomes 
similar  to  a  sequential  target  interception  problem. 
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// 

UAV^ 

Figure  4.3:  Intersection  of  safety  boundaries  of  two  obstacles 

4.2  Geometry  of  the  Guidance  Problem 

The  guidance  objective  is  to  align  the  velocity  vector  to  the  aiming  line  so  that  collision  is 
avoided.  The  aiming  line  is  the  line  joining  the  aiming  point  X  to  the  center  of  gravity 

of  the  vehicle,  in  the  case  of  stationary  obstacles.  The  problem  geometry  is  shown  in 
Figure  4.4.  u ,  v  and  w  are  the  velocity  components  along  the  x,  y  and  z  directions 
respectively,  and  a  =v,  a.=w  are  the  accelerations  applied  in  the  y  and  z  directions 

respectively.  Note  that  the  velocity  u  is  assumed  to  be  a  constant. 

The  angle  6  between  the  total  velocity  vector  and  the  aiming  line  is  to  be  eliminated. 
In  order  to  formulate  a  guidance  which  computes  the  controls  a  and  a.  that  would 

eliminate  6 ,  we  consider  the  3D  problem  as  a  combination  of  two  separate  2D  problems 
in  the  XY  and  XZ  planes.  (Xap)XY  and  (T  )K  are  the  projections  of  the  aiming  line  on 
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to  the  XY  and  XZ  planes  respectively,  while  VXY  and  V x/  are  projections  of  the  velocity 
vector  on  to  the  XY  and  XZ  planes  respectively.  The  orthogonal  projection  Vp  of  a  vector 
V  onto  a  plane  with  basis  vectors  Xx  and  X2  is  [27] 

Vp  =  c\Xx  +  c2X2  (4.7) 


X 


u 


q  and  c2  are  found  as  follows: 


=  [(XTX)~1XT]V 


(4.8) 


x  —  \x  x  lr 

where  I-  1  2  -I  .  The  basis  vectors  in  the  problem  under  consideration  are  the  unit 

vectors  along  the  X,  Y  and  Z  axes  i.e. 
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(4.9) 
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0  is  the  angle  between  (X  )XY  and  V XY  and  6  is  the  angle  between  (Xap)x/  and 
V-^.  The  guidance  objective  is  restated  as:  compute  the  controls  a  and  a.  to  eliminate 
6y  and  6.  respectively  in  the  two  independent  XY  and  XZ  planes. 

4.3  2D  Decomposition  of  the  3D  Guidance  Problem 

Figure  4.5  shows  the  geometry  of  the  guidance  problem  in  the  XY  plane.  Due  to 
symmetry,  the  geometry  of  the  problem  in  XZ  plane  is  the  same. 


a(o,o) 


b(«,o) 


v,a 


Figure  4.5:  Geometry  of  the  guidance  problem  in  2D 


When  the  velocity  vector  VXY  is  perfectly  aligned  with  the  aiming  line  X ap ,  the  y- 

component  of  the  velocity  vector  changes  to  a  new  value,  v*  ( w*  in  the  XZ  plane).  Note 
that  in  keeping  with  the  kinematics  of  the  UAV,  the  x-velocity  u  remains  constant,  v*  is 
found  by  the  intersection  of  the  lines  /,  and  /2.  The  points B(u,0) ,  E(u,v  )  and  C(u,v ) 
lie  on  the  line  /,  i.e., 


/,  :x  =  u  (4.10) 

The  points ^4(0, 0) ,  E(u,v* )  and  X  (x  , y  )  lie  on  the  line  l2.  The  equation  of  l2 

using  the  two-point  form  of  equation  of  the  line  is: 


y 


ap 


X 


(4.11) 
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The  point  E  is  the  intersection  of  the  lines  /,  and  l2.  We  substitute  (4.10)  into  (4.1 1) 
in  order  to  find  v* ,  the  y-coordinate  of  E 

(4.12) 

Since  u  is  considered  to  be  a  constant  and  Xap  is  a  constant  point,  v*  at  each  instant. 


4.4  Guidance  Strategy 

This  section  describes  two  guidance  strategies  used  to  navigate  the  UAV  in  order  to  avoid 
collision.  The  two  new  guidance  strategies  are  Differential  Geometric  Guidance  (DGG) 
and  Nonlinear  Geometric  Guidance  (NGG)  proposed  in  [4], 


4.4.1  Nonlinear  Geometric  Guidance 

The  Nonlinear  Geometric  Guidance  (NGG)  law  is  as  follows: 


a  1  k  sin  6* 

y  —  y 

a*\  \_Kv  sin  _ 


(4.13) 


The  control  is  thus  a  nonlinear  function  of  the  aiming  angle  6 .  An  advantage  that 
immediately  presents  itself  is  that  the  range  of  the  sine  function  is  [-1, 1]  whereas  the 
range  of  0  is  [-go,  co]  .  This  indicates  that  the  acceleration  in  NGG  is  always  bounded, 


provided  kv  is  bounded. 


The  gains  kv  and  kw  can  be  selected  as  constants  with  some  tuning.  However,  it  is 


not  a  good  idea  to  have  constant  gains,  because  when  t  is  low,  high  control  needs  to  be 


generated  and  vice-versa.  Hence  control  gains  are  set  inversely  proportional  to  t  as 
given  by  following  equation: 


a 
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here  a ,  b ,  a  and  ft  are  tuning  parameters  which  are  set  to  following  values  through 
trial  and  error. 

a  =  4  a-  0.6 
b  =  4  p  =  0.6 

4.4.2  Nonlinear  Differential  Geometric  Guidance 

In  order  to  achieve  alignment  of  the  velocity  vector  with  the  aiming  line,  the  control  ay 

should  be  designed  so  that  the  y-velocity  v  approaches  v*  in  time  t  .  The  nonlinear 

Differential  Geometric  Guidance  (DGG)  is  based  on  Dynamic  Inversion  (DI)  [28,  29],  a 
control  strategy  used  for  output  tracking  of  nonlinear  systems.  The  principle  of  DI  is  to 
drive  a  stabilizing  error  dynamics  (chosen  by  the  designer)  to  zero.  The  main  advantage 
of  DI  is  that  it  essentially  guarantees  global  asymptotic  stability  with  respect  to  the 
tracking  error. 

We  now  describe  the  DI  guidance  design.  Let  the  error  be 


* 

ev  =  V-V 

(4.14) 

Imposing  the  first  order  error  dynamics 

ev+kvev  =  0 

(4.15) 

i.e. 

(v-v*)  +  kv(v-v*)  =  0 

(4.16) 

With  quasi-steady  approximation  (i.e.  assuming  u  is  a  constant  at  every  instant  of 
time),  v*  is  a  constant.  In  addition  v  =  av,  from  the  system  dynamics.  The  DI  based 
guidance  law  is  hence  derived  to  be: 

ay=-kv(v-v )  (4.17) 

We  call  the  expression  of  a  in  (4.17)  the  “Dynamic  Inversion  Guidance”  law,  or  the 

“Nonlinear  Differential  Geometric  Guidance”  (DGG)  law,  since  the  guidance  strategy  is 
derived  based  on  the  derivative  of  the  error.  The  constant  kv  is  designed  such  as  the 
settling  time  (i.e.  the  time  taken  to  align  the  velocity  vector  with  the  aiming  line)  is 

inversely  proportional  to  the  t  ,  i.e.  kv  =—  where  rv  is  the  desired  time  constant  of  the 
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error  dynamics.  Note  that  rv  can  be  selected  by  choosing  an  appropriate  settling  time  TSv 

T 

,  since  for  linear  systems  theory,  rv  =  — . 

Furthermore  one  can  choose  TSv  as  TSv  =  atgo ,  0  <  a  <  1 .  Such  a  guidance  strategy 
ensures  that  a  larger  control  is  generated  for  an  obstacle  that  is  nearer  (i.e.  the  tgo  is 

smaller).  The  guidance  strategy  in  (4.17)  is  proportional  to  the  error  in  the  y-velocity,  and 
thus  produces  a  large  control  input  at  the  beginning  which  effects  quick  settlement  along 
the  aiming  line.  Two  factors  influence  the  control  av : 

•  t  :  The  gain  kv  is  designed  to  be  inversely  proportional  to  t  so  that  larger 

control  is  demanded  for  obstacles  that  are  closer. 

•  a  :  The  choice  of  a  detennines  the  speed  of  settling  of  the  control.  If  the  value  of 
a  is  close  to  1,  the  settling  is  slow  and  consequently,  the  peak  in  control  is  low. 
However  if  fast  settling  is  desired,  a  high  value  of  a  will  effect  this.  However 
faster  settling  results  in  a  higher  peak  in  control. 

If  both  the  t  and  a  are  small,  the  peak  in  control  may  become  unfeasibly  large  and 

result  in  control  saturation.  The  value  of  a  must  therefore  be  chosen  judiciously  based 
on  the  requirement  of  speed  of  alignment.  Since  reactive  collision  avoidance  requires 
quick  maneuvering,  a  value  of  a  between  0.3  and  0.8  are  suitable.  Slower  maneuvers 
with  a  >  0.8  may  be  suitable  for  flying  to  the  destination. 

In  an  analogous  manner,  the  expression  for  a z  in  XZ  plane  can  be  derived  as 

ciz  =-kK{w-w)  (4.18) 

*„=—  (4.19) 

Tv 

and 

4v  =  4rw  (4.20) 

where 

TSw=Ptgo  (4.21) 

We  examine  the  stability  of  the  Dl-based  guidance  strategy  with  respect  to  the  error. 
The  error  is  defined  as: 
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The  error  dynamics  are  described  as  follows: 


(4.22) 


(4.23) 


Equation  (4.23)  constitutes  a  linear  time -varying  system.  Consider  a  linear  time- 
varying  system  of  the  fonn  x  =  A(t)x .  The  system  is  determined  to  be  asymptotically 

stable  if  the  symmetric  matrix  A(t)  +  Ar(t)  has  all  eigen  values  lying  the  left  half  of  the 
complex  plane  [30].  In  the  system  in  (4.23),  the  eigen  values  of  A(t)  +  AT (t)  are  -2 kv 


and  -2 kw .  Since  kv  and  kw  are  inversely  proportional  to  the  t  ,  they  are  strictly  positive. 

Therefore  both  the  eigen  values  lie  strictly  on  the  left-half  of  the  complex  plane,  and  the 
system  in  (4.23)  is  asymptotically  stable. 


4.4.3  Correlation  of  DGG  and  NGG 

The  DGG  law  is  equivalent  to  the  NGG  law,  if  control  gains  kv  and  kw  are  set  as  given 
by  following  formula. 


(5.7) 


here  V  =  [u  v  w]r  is  the  UAV  velocity  vector,  v*  and  w  are  desired  Y  velocity  and  Z 
velocity  respectively. 


With  above  gain  settings  for  DGG,  the  controls  generated  by  it  will  be  exactly  same 
as  controls  generated  by  NGG  law.  Thus  both  guidance  strategies  are  directly  correlated. 
The  NGG  will  therefore  possess  all  the  advantages  of  DGG  discussed  in  previous  section. 
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Chapter  5 
Results 


The  EKF  based  estimation  technique  developed  in  this  study  is  tested  in  a  number  of 
numerical  experiments  for  two  scenarios  i.e.  Single  Obstacle  with  Target  Estimation  and 
Two  Obstacles  with  Target  Estimation  in  3D  separately  for  each  of  the  guidance  strategy. 
Additionally  to  check  the  consistency  of  the  EKF,  the  Sigma-bound  test  was  performed. 
The  results  of  Sigma-bound  test  are  also  presented  in  this  Chapter. 

5.1  Success  Criterions 

The  success  of  the  algorithm  was  tested  on  three  criterions: 

•  Violation  of  the  safety  sphere 

•  Divergence  from  the  safety  sphere 

•  UAV’s  Target  Miss  Distance 

Important  thing  to  note  here  is  that  while  our  primary  objective  is  to  avoid  the 
obstacle,  at  the  same  time  UAV  should  not  diverge  too  much  from  its  path  in  the  process. 
If  the  estimate  of  the  obstacle  position  is  good  then  UAV’s  closest  approach  with  the 
obstacle  should  be  roughly  equal  to  the  radius  of  the  safety  sphere,  since  obstacles  appear 
almost  at  the  direct  path  between  start  point  and  destination.  Both,  too  much  violation  of 
safety  sphere  and  too  much  divergence  from  it,  indicate  that  obstacle  position  estimates 
were  not  good  enough. 

The  final  success  criterion  represents  how  close  UAV  gets  to  the  target.  To  calculate 
the  UAV’s  target  miss-distance,  a  second  order  curve  was  fitted  among  closest  approach 
point,  one  before  it  and  one  after  it.  Following  equations  show  how  target  miss-distance 
was  calculated. 
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(5.1) 


R{tf  -  At)  =  a  +  b  (tf  -  At)  +  c(tf-  At)2 
R{tf)  =  a  +  b(tf)  +  c  ( tf )2 
R(tf  +  At)  =  a  +  b  ( tj-  +  At)  +  c  (t y  +  A?)" 

here  /?(?)  is  the  distance  of  the  UAV  from  the  target  at  time  t .  a ,  b  and  c  are 
coefficients  of  a  second  order  function  and  calculated  by  following  equations. 


R(tf-At) 

R(tf) 

= 

R(tf  +  At) 

1  (tf-At)  (tf-Aty 
1  (tf)  (tf)2 
1  (t  r  +At)  (t,+At)_ 


(5.2) 


a 

1  (t  f  -  At)  (tf  -  At)2 

-1 

R(tf  -At) 

b 

= 

1  (tf)  (tf)2  1 

R(tf) 

c 

1  (ty+At)  (ty+At)’ 

R(tf  +  At) 

After  computing  a ,  b  and  c ,  the  time  of  UAV’s  closest  approach  tmm 
(5.4)  and  then  target  miss-distance  is  calculated  using  (5.5). 


(5.3) 


calculated  by 


b_ 

2c 


R  (t  )  =  a  +  b(t  )  +  c(t  )2 

minv^min/  V^min  '  V  min/ 


(5.4) 

(5.5) 


Based  on  these  success  criterions,  different  segments  of  success  are  created  defined 
by  the  band  of  the  closest  approach  of  the  UAV  with  obstacles  and  target.  These 
segments  of  success  are  named  as  S-l,  S-2,  S-3  and  S-4  where  each  increment  represents 
slightly  relaxed  success  conditions  i.e.  width  of  the  tolerable  closest  approach  band  is 
increased  so  S-l  represents  the  strictest  conditions  while  S-4  represents  most  relaxed 
case.  These  conditions  are  described  in  Table  I. 


TABLE  I:  Different  Success  Bands 


Success 

band 

Tolerable  Safety  Sphere 

Violation  (as  %  of  the 

Safety  Sphere  Radius) 

Tolerable  Divergence  from 

Safety  Sphere  (as  %  of  the 

Safety  Sphere  Radius) 

UAV’s  Target  Miss 

Distance  (m) 

S-l 

10% 

10% 

<  10 

S-2 

20% 

20% 

<  10 

S-3 

30% 

30% 

<  10 

S-4 

40% 

40% 

<  10 
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5.2  Single  Obstacle  with  Target  Estimation 

The  experiments  with  a  single  stationary  obstacle  involve  a  finite  space  with  one  point 
obstacles  with  a  known  safety  sphere  radius.  The  position  of  the  obstacle  is  randomly 
chosen  in  each  simulation  run  making  sure  it  obstructs  the  path  of  the  UAV.  The  initial 
velocity  of  the  UAV  is  also  chosen  randomly  uniformly  distributed  between  following 
limit  (in  meters  per  second). 

5  <  u  <  20 

-5  <  v  <  5  (5.6) 

-5  <  w  <  5 

For  first  10  seconds  of  the  simulation,  no  control  is  applied  and  EKF  estimates  the 
obstacle  and  target  positions  in  an  open  loop  system  (pre-run).  After  the  pre-run,  EKF 
reinitializes  the  state  vector  and  error  covariance  and  then  system  applies  one  of  the  two 
navigation  law  in  order  to  find  a  collision  free  path.  Figure  5.1  shows  an  example 
scenario  of  3D  space  where  simulations  are  being  conducted. 


Figure  5.1  Finite  3D  Space  with  One  Obstacle 

5.1.1  DGG  Navigation  Strategy 


In  this  part  of  the  experiments,  DGG  navigation  law  was  applied  in  order  to  find  a 
collision  free  path.  A  total  of  1000  simulation  runs  performed  in  order  to  test  the 
effectiveness  of  the  DGG  navigation  law  while  estimating  the  obstacle  and  target  position 
with  EKF.  Based  on  the  success  criterion  described  in  section  5.1,  following  Table  II 
shows  the  percentage  of  successes. 
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TABLE  II:  Success  Percentage  with  DGG  Navigation 


Success  band 

%  Run  Satisfying  the  Success  Conditions 

S-l 

51.3 

S-2 

73.8 

S-3 

84.1 

S-4 

90.5 

The  Figure  5.2  shows  the  UAVs  closest  approach  with  obstacle  as  the  Percentage  of  the 
safety  sphere  radius.  In  Figure  5.2  different  color  lines  represent  the  different  success 
bands.  The  Figure  5.3  shows  the  final  distance  of  UAV  from  the  destination.  In  this 
Figures,  Black  dots  represent  the  successful  case  while  Red  dots  represent  the  failures. 


Figure  5.2:  UAV’s  Closest  Approach  to  Obstacle  with  DGG  Navigation 
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Figure  5.3:  UAV’s  Target  Miss-Distance  with  DGG  Navigation 


5.1.2  NGG  Navigation  Strategy 

Here  NGG  navigation  law  was  applied  in  order  to  find  a  collision  free  path.  Again  a  total 
of  1000  simulation  runs  perfonned  in  order  to  test  the  effectiveness  of  the  NGG 
navigation  law  while  estimating  the  obstacle  and  target  position  with  EKF.  Based  on  the 
success  criterion  described  in  section  5.1,  following  Table  III  shows  the  Percentage  of 
successes. 


TABLE  III:  Success  Percentage  with  NGG  Navigation 


Success  band 

%  Run  Satisfying  the  Success  Conditions 

S-l 

50.1 

S-2 

72.5 

S-3 

84.1 

S-4 

91.2 

The  Figure  5.4  shows  the  UAVs  closest  approach  with  obstacle  as  the  Percentage  of  the 
safety  sphere  radius.  The  Figure  5.5  shows  the  final  distance  of  UAV  from  the 
destination. 
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UAV’s  closest  approach  from 

UAV  s  Target  Miss-Distance  Obstacle  as  %  of  safety  radius 
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Figure  5.4:  UAV’s  Closest  Approach  to  Obstacle  with  NGG  Navigation 


Figure  5.5:  UAV’s  Target  Miss  Distance  with  NGG  Navigation 


5.2  Two  Obstacles  with  Target  Estimation 

The  experiments  with  two  stationary  obstacles  involve  a  finite  space  with  two  point 
obstacles  with  known  safety  sphere  radiuses.  The  positions  of  the  obstacles  are  randomly 
chosen  in  each  simulation  run  while  making  sure  they  obstruct  the  path  of  the  UAV 
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between  the  start  point  and  the  destination.  The  initial  velocity  of  the  UAV  is  also  chosen 
randomly  uniformly  distributed  between  following  limit  (in  meters  per  second). 

5  <  u  <  20 

-5  <  v  <  5  (5.7) 

-5  <  w  <  5 

For  first  10  seconds  of  the  simulation,  no  control  is  applied  and  EKF  estimates  the 
obstacle  and  target  positions  in  an  open  loop  system  (pre-run).  After  the  pre-run,  EKF 
reinitializes  the  state  vector  and  error  covariance  and  then  system  applies  one  of  the  two 
navigation  law  in  order  to  find  a  collision  free  path.  Figure  5.6  shows  an  example 
scenario  of  3D  space  where  simulations  are  being  conducted. 


5.2.1  DGG  Navigation  Strategy 

In  this  part  of  the  experiments,  DGG  navigation  law  was  applied  in  order  to  find  a 
collision  free  path.  A  total  of  1000  simulation  runs  perfonned  in  order  to  test  the 
effectiveness  of  the  DGG  navigation  law  while  estimating  two  obstacles  and  target 
positions  with  EKF.  Based  on  the  success  criterion  described  in  section  5.1,  following 
Table  IV  shows  the  Percentage  of  successes. 


TABLE  IV:  Success  Percentage  with  DGG  Navigation 


Success 

band 

%  Run  Satisfying  the 

Success  Conditions  for 

Obstacle  1 

%  Run  Satisfying  the 

Success  Conditions  for 

Obstacle  2 

%  Run  Satisfying  the  Success 

Conditions  for  Both  Obstacles 

Simultaneously 

S-l 

48.6 

51.9 

31.4 

S-2 

68.8 

75 

54.6 
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UAV’s  closest  approach  from 
Obstacle  1  as  %  of  safety  radius 


S-3 

82.5 

88.5 

74.3 

S-4 

88.5 

95.6 

84.3 

The  Figure  5.7  shows  the  UAVs  closest  approach  with  obstacle  1  as  the  Percentage  of  the 
safety  sphere  radius.  Similarly  Figure  5.8  shows  the  UAVs  closest  approach  with  obstacle 
2  as  the  Percentage  of  the  safety  sphere  radius.  The  Figure  5.9  shows  the  final  distance  of 
UAV  from  the  destination. 


Figure  5.7:  UAV’s  Closest  Approach  to  Obstacle  1  with  DGG  Navigation 
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UAV’s  closest  approach  from 

UAV  s  Target  Miss-Distance  Obstacle  2  as  %  of  safety  radius 


Figure  5.8:  UAV’s  Closest  Approach  to  Obstacle  2  with  DGG  Navigation 


Number  of  Simulation  Runs 

Figure  5.9:  UAV’s  Target  Miss-Distance  with  DGG  Navigation 


5.2.2  NGG  Navigation  Strategy 

In  this  part  of  the  experiments,  NGG  navigation  law  was  applied  in  order  to  find  a 
collision  free  path.  A  total  of  1000  simulation  runs  perfonned  in  order  to  test  the 
effectiveness  of  the  NGG  navigation  law  while  estimating  two  obstacles  and  target 
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UAV’s  closest  approach  from 


positions  with  EKF.  Based  on  the  success  criterion  described  in  section  5.1,  following 
Table  V  shows  the  Percentage  of  successes. 


TABLE  V :  Success  Percentage  with  NGG  Navigation 


Success 

band 

%  Run  Satisfying  the 

Success  Conditions  for 

Obstacle  1 

%  Run  Satisfying  the 

Success  Conditions  for 

Obstacle  2 

%  Run  Satisfying  the  Success 

Conditions  for  Both  Obstacles 

Simultaneously 

S-l 

46.8 

55.3 

27.6 

S-2 

79.3 

70 

57.5 

S-3 

80.4 

90.5 

73.9 

S-4 

87.8 

96.4 

84.7 

The  Figure  5.10  shows  the  UAVs  closest  approach  with  obstacle  1  as  the  Percentage  of 
the  safety  sphere  radius.  Similarly  Figure  5.11  shows  the  UAVs  closest  approach  with 
obstacle  2  as  the  Percentage  of  the  safety  sphere  radius.  The  Figure  5.12  shows  the  final 
distance  of  UAV  from  the  destination. 


Number  of  Simulation  Runs 

Figure  5.10:  UAV’s  Closest  Approach  from  Obstacle  1  with  NGG  Navigation 
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UAV’s  closest  approach  from 

UAV  s  Target  Miss-Distance  Obstacle  as  %  of  safety  radius 


Figure  5.11:  UAV’s  Closest  Approach  from  Obstacle  2  with  NGG  Navigation 


Figure  5.12:  Distances  between  Destination  and  UAV’s  Final  Location  with  NGG 

Navigation 


5.3  Sigma  Bound  Test 

It  is  important  to  perform  the  consistency  check  while  using  EKF.  Sigma  bound  test  is 
one  such  test  which  checks  whether  EKF  is  behaving  close  to  what  is  theoretically 
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Elevation  (deg)  Azimuth  (deg)  Range  (m) 


expected.  During  the  simulation  runs  of  system,  Sigma-bound  test  was  also  perfonned  in 
order  to  check  if  error  in  state  estimates  lies  within  the  standard  deviation  given  by  the 
error  covariance  matrix  P.  Figure  5.13  shows  the  Sigma-bound  test  for  obstacle 
estimation  in  one  of  the  simulation  run. 


40 


-20 


-40 - 1 - 1 - 1 - 1 - 1 - 1 - 1 

5  io  15  20  Time  25  30  35  40 

Figure  5.13:  One  Sigma  Bound  Test 


With  each  simulation  run,  Sigma  bound  test  was  performed  i.e.  estimation  error  in  state 
element  is  compared  with  the  square  root  of  the  corresponding  diagonal  element  of  the  P 
matrix.  At  the  same  time  estimation  errors  are  also  compared  with  two  times  and  three 
times  of  the  error  standard  deviation.  Following  Tables  VI  to  IX  show  the  results  of 
Sigma-bound  test  in  terms  of  percentage  of  estimation  errors  bounded  by  1 -Sigma,  2- 
Sigma  and  3 -Sigma  bounds  for  each  set  of  the  simulation.  Results  are  given  is  average 
terms  i.e.  after  running  1000  simulations,  the  Sigma  test  results  are  averaged  for  all 
simulations. 
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Table  VI:  Single  Obstacle  with  Target  Estimation  while  Applying  DGG  Navigation 


State 

Average  %  of  the 

Average  %  of  the 

Average  %  of  the 

Vector 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Elements 

by  1-Sigma 

by  2-Sigma 

by  3-Sigma 

rob 

66.05 

94.24 

98.97 

0ob 

64.18 

95.67 

99.35 

hob 

63.92 

96.57 

98.21 

Vtr 

77.73 

91.44 

94.83 

0lr 

68.59 

94.46 

98.68 

hr 

69 

94.49 

98.76 

Table  VII:  Single  Obstacle  with  Target  Estimation  while  Applying  NGG  Navigation 


State 

Average  %  of  the 

Average  %  of  the 

Average  %  of  the 

Vector 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Elements 

by  1-Sigma 

by  2-Sigma 

by  3-Sigma 

rob 

65.95 

91.85 

97.05 

0ob 

62.65 

96.  34 

97.73 

hob 

63.02 

96.93 

99.25 

Vtr 

77.06 

91.89 

94.5 

0,r 

69.22 

94.26 

98.65 

hr 

68.14 

94.28 

98.79 
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Table  VIII:  Two  Obstacles  with  Target  Estimation  while  Applying  DGG  Navigation 


State 

Average  %  of  the 

Average  %  of  the 

Average  %  of  the 

Vector 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Elements 

by  1-Sigma 

by  2-Sigma 

by  3-Sigma 

robX 

65.98 

93.06 

99.46 

Oob\ 

66.56 

89.93 

96.69 

fiobl 

65.32 

90.63 

96.31 

robl 

70.28 

96.43 

99.18 

^obl 

64.62 

89.87 

95.33 

*) 'obi 

63.74 

89.72 

95.37 

Vtr 

78.24 

93.45 

96.60 

0* 

68.90 

95.03 

99.34 

k 

67.92 

94.64 

99.31 

Table  IX:  Two  Obstacles  with  Target  Estimation  while  Applying  NGG  Navigation 


State 

Average  %  of  the 

Average  %  of  the 

Average  %  of  the 

Vector 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Estimation  Error  Bounded 

Elements 

by  1-Sigma 

by  2-Sigma 

by  3-Sigma 

robX 

67.98 

86.710 

93.93 

Oobl 

64.36 

96.56 

99.58 

fiobl 

62.56 

84.21 

91.49 

robl 

62.41 

89.63 

95.81 

6 obi 

61.38 

94.83 

99.14 

^obl 

68.58 

95.69 

99.49 

Vtr 

78.32 

93.39 

96.81 

0lr 

66.38 

94.81 

99.08 

t 

69.16 

94.82 

98.85 
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The  Sigma  bound  tests  whether  the  error  covariance  matrix  is  representing  the  estimation 
errors  as  theoretically  expected.  The  diagonal  elements  of  the  P  matrix  correspond  to  the 
estimation  error  variance  of  the  individual  state  vector  elements.  Theoretically,  the 
estimation  errors  should  be  normally  distributed  with  covariance  given  by  the  P  matrix. 
If  estimation  errors  are  normally  distributed,  they  should  be  according  to  the  following 
empirical  law  of  normal  distribution: 

•  Percentage  of  estimation  errors  bounded  by  1 -Sigma:  68.26 

•  Percentage  of  estimation  errors  bounded  by  2-Sigma:  95.44 

•  Percentage  of  estimation  errors  bounded  by  3-Sigma:  99.74 

It  is  clear  from  above  tables  that  EKF  developed  in  this  algorithm  is  following  these 
bonds  with  a  satisfactory  degree.  This  indicates  that  EKF  is  performing  as  theoretically 
expected  with  good  convergence. 
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Chapter  6 
Conclusions 


A  solution  to  the  mission-critical  problem  of  reactive  collision  avoidance  of  UAVs  is 
presented  in  this  paper.  Two  newly  developed  guidance  strategies  which  are  highly  suited 
to  solve  the  problem,  i.e.  NGG  and  DGG  are  validated  in  this  paper  with  vision  based 
sensing.  The  effectiveness  of  the  two  guidance  laws  in  the  presence  of  the  visual 
information  is  demonstrated  in  the  simulation  results  to  problem  scenarios  with  stationary 
obstacles  in  3D. 

The  vision  based  sensing  is  one  of  most  focused  research  area  in  UAV  automation 
because  of  its  many  significant  advantages.  Additionally  in  nature,  most  of  the  birds  and 
insects  apply  vision  based  sensing  as  most  effective  tool  for  navigation  and  detection. 
The  significance  of  guidance  strategies  implemented  here  for  collision  avoidance  lies  in 
their  emphasis  on  rapid  alignment,  in  contrast  with  existing  guidance  laws.  Since 
collision  avoidance  is  a  potentially  mission-critical  component,  specifications  such  as  low 
control  effort,  minimum  fuel  consumption  etc.  are  not  as  crucial  as  the  safety  of  the 
vehicle. 

A  possible  extension  to  the  ideas  presented  in  this  paper  is  the  problem  of  collision 
avoidance  with  moving  and  maneuvering  obstacles  such  as  other  UAVs  (e.g.,  urban 
warfare  situation).  Note  that  the  use  of  the  vision  based  sensing  is  not  limited  to  only 
collision  avoidance.  Other  potential  applications  would  include  guidance  of  missiles  and 
guided  munitions  since  the  obstacles  can  in  fact  be  the  real  target.  So  instead  of  avoiding 
them,  objective  could  be  to  intercept  them. 

Additionally,  an  effort  has  been  made  into  developing  an  effective  vision  based 
collision  avoidance  technique;  some  of  the  assumptions  made  in  this  study  (such  as 
stationary  point  obstacles,  perfect  infonnation  about  UAV’s  position  and  velocity, 


62 


availability  of  assumed  image  processor,  kinematic  model)  are  not  realistic  and  hence 
needs  to  be  relaxed  in  future  studies. 
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Appendix  A 

Derivation  of  the  Expressions  for  Criteria  of  Criticality  and 
Aiming  Point  in  the  Collision  Cone  Approach 


From  Figure  4.1  we  note  that  r  and  uj  are  perpendicular,  giving  the  following  results: 

ri-ui  =  {Xr  +  dui)-ui=  0  i  =  1,2  (Al) 

Since  uj  ■  ut  =  1  we  have: 

Xr-u,+d  =  0  (A2) 

The  vector  uf  is  defined  in  a  2D  space  spanned  by  Xr  and  V  and  can  therefore  be 
expressed  as 

u,  =  aXr  +  pV  (A3) 

where  ai  and  /?.  are  scalar  coefficients.  Using  this  in  (A2), 

V  •  (aXr  +  py)  +  d  =  cci  1 1  X,.  1 12  +pi (A  •  V)  +  d  =  0 


a,  =  - 


Pl(Xr-V)  +  d 
II  A  .II2 


Further,  ut  •  w.  =  I .  therefore  from  (A3), 


{aiXr+PiV)-{aiXr+py)  =  at  \\Xr  ||2  +2a^(Ar-F)  +  ^  11*1=1 

Substituting  (A4)  in  (A6): 

{P,{Xr-V)  +  df  _2{Pl{Xr-V)  +  d)P,{Xr-V)  ,  /?2|IT/I|2 


A. 


Solving  for  /? , 


A=±. 


Ur 

II2 

II 

\Xr  II 

I2  -d2 

\\xr\\2 

11*1 

2  ~(X,.-V)2 

=  ±c 


Let  f\  =  +c  and  /?2  =  -c ,  from  (A3)  and  (A4) 


1 


Mi  =  —  FU 


X: 


1 


-(c(Xr-V)  +  d)Xr+cV 


«,  = - -{c{X-V)-d)X-cV 

2  II  X2  ,,V  , 


(A4) 

(A5) 

(A6) 


+  A  11*1=1  (A7) 


(A8) 


(A9) 

(A10) 
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xr-r 

xr  ||2  - d 2 


(A19) 


(A  18)  and  (A  19)  give  the  expressions  for  a  and  b  ,  used  to  detennine  the  criticality  of  an 
obstacle.  The  unit  vectors  w,  and  u2  in  (A9)  and  (A  10)  are  used  for  aiming  point 
computation. 
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Appendix  B 

Derivation  of  the  Virtual  Aiming  Point  in  the  Sphere  Tracking 
Algorithm  for  Tracking  Safety  Sphere 


The  objective  of  the  sphere  tracking  algorithm  is  to  compute  a  virtual  aiming  point.  The 
virtual  aiming  point  is  a  point  on  the  surface  of  the  safety  sphere  and  is  the  intersection  of 
Xr  and  the  safety  sphere  (see  Figure  4.2).  The  equation  of  the  line  Xr  in  the  two-point 
form  is: 


x  —  x 


ob 


y-yob 


z- z 


ob 


X.,  -X 


ob 


Z„  -Z 


=  k 


(Bl) 


ob 


yv-yob 

where  k  is  the  constant  of  proportionality.  Further,  the  equation  of  the  safety  sphere  is: 

(x-xobY +(y-yobf +(z-zobY  =d2  (B2) 


We  wish  to  find  the  intersection  of  the  relative  distance  line  with  the  safety  sphere. 
Therefore,  substituting  (Bl)  in  (B2)  yields  the  following  equation: 

k2(xv-xob)2  +k2(yv-yobf  +k\zv-zobf  =d2  (B3) 

k=±  .  d  (B4) 

v  (A  -Xobf + (A  -yob)2  +  (A  -  zoi ) 


The  virtual  aiming  point X  (x,y,z)  is  computed  from  (Bl)  by  substituting  for  k  : 

Xvap=k(Xv-Xob)  +  Xob  (B5) 

yVaP=k(yv-yob)+yob  (B6) 

ZVaP  =  k(ZV-Zob)  +  Zob  (B6) 


There  are  two  solution  sets  for  Xvap  depending  on  the  sign  of  k  .  Note  that  in  the  above 

equations  for  the  x,  y  and  z  coordinates,  the  value  of  k  may  either  be  uniquely  positive  or 
uniquely  negative  (from  (B4)).  The  solution  chosen  is  the  point  closer  to  the  vehicle's 
position  Xv  . 
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Appendix  C 

Vision  Based  Obstacle  Avoidance  Algorithm 

The  step-by-step  algorithm  implemented  in  the  numerical  simulations  is  given  below.  For 
simplicity  algorithm  presented  here  is  for  position  estimation  of  only  one  object.  It  can  be 
easily  augmented  for  simultaneous  position  estimation  of  multiple  objects  by  augmenting 
the  state  vector  and  EKF  accordingly. 

Step  1:  After  getting  the  first  processed  image  from  the  image  processor,  initialize  the 
state  vector  Xr(0)  according  to  number  of  objects  found. 

V(0)  =  [fo6(0)  dob(0)  ^(0)]r 

here  foZ,(0)  is  given  with  50%  uncertainty  assuming  some  prior  information,  while 
0oh{ 0)  and  <j)ob{ 0)  are  given  by  first  measurements. 


Step  2:  Initialize  the  Error  Covariance  Matrix  P0 : 


P0  =  diag  axe  a 


r0xWv 


ro x  Wv 


here  e  is  the  maximum  range  measurement  uncertainty  (in  meters)  given  by  the  50% 
of  the  distance  between  the  start  point  and  the  destination,  r0  =  20  is  maximum 

percentage  measurement  noise.  al  =  1 ,  a2=  2  and  a3=  2  are  tuning  parameters.  Wv 

2 

is  camera’s  width  of  view  (in  radians)  assumed  —  n  . 


Step  3:  Initialize  the  Process  Noise  Covariance  Q : 

Q  =  diag  (0.2  0.025  0.025) 


Step  4:  Run  EKF  in  open  loop  system  for  10  Seconds  (Pre-run) 
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4. 1 :  Propagate  the  State  Vector  X*  — >  V.  : 


w  cos  0oA  cos 4,  +  v sin  4  cos  4  +  rvsin  (f>ob 

sin  4  cos  <9 ' 

- n  + - v 

Kb  COS</>ob  'ob  COS  fob 


cos  sin  <j)ob  _  sin  0ob  sin  </>ob  cos  _ 
- n - v  h - rv 


'oft 


'06 


4.2:  Propagate  the  Covariance  Matrix  /)' ,  — »  Pk  : 

P(t)  =  A(t)P(t)  +  P(t)AT(t)  +  Q 

here  ^4(7)  is  calculated  by  using  expression  in  equation  (3.35). 
4.3:  Compute  Measurement  Error  Covariance  Matrix  Rk : 


Rk  =  diag 


(  wv  iV  (  wv  n 


obu 


-X  — 


100  3 


' Ob, 


-X  — 


100  3 


here  roh  is  given  by  the  equation  (3.25)  from  range  dependent  measurement  noise 
model. 


4.4:  Compute  Kalman  Filter  Gain  Kk : 

K^PbCllCMl+R,] 
8h\ 


here  C,  =  ■ 


dX 


"0 

i 

0" 

rk(,) 

0 

0 

1 

4.5:  Take  the  Measurement  Yk : 

Note  that  here  X  is  the  actual  value  of  the  state  vector  and  v,  is  the 

rk  k 

measurement  noise  vector. 


0  1  0 
0  0  1 
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4.6:  Update  the  State  Vector  X r  — >  Xr  : 


x:  =X~  +K, 


Y,  -  h{X~  ) 


4.7:  Update  the  Covariance  Matrix  Pk  — >  i^+  : 


4.8:  For  first  10  seconds,  at  every  grid  point  of  time,  repeat  steps  4. 1  to  4.7. 

Step  5:  Reinitialize  the  State  Vector  Xr(0) 

1  N  „ 

V(0)  =  T7Z^('') 

tv  ;=1 

here  V  is  the  number  of  estimations  made  by  EKF  during  pre-run  (Step  4). 


Step  6:  Reinitialize  the  Error  Covariance  Matrix  P0 : 


P0  =  tliag 


r0  xWv 

100 


o  * 
A2 


fp  X  Wy 

100 


Step  7:  Run  EKF  with  Guidance  in  Closed  Loop  System  till  the  UAV  crosses  the  Target 
5.1 :  Repeat  steps  4. 1  to  4.7  i.e.  Estimate  the  Obstacle  or  Target  Position. 

5.2:  Perform  the  Smoothing  Operation 


1  k 

x-  =-  I  c 


n  /=, 


i=k—n— 1 


here  X r  is  smoothed  estimate,  X r  original  estimate  at  time  instant  k  and 


n  =  10 . 


5.3:  If  Estimated  Object  is  Obstacle: 


-  Check  for  collisions  (Apply  Collision  Cone  approach  given  in  Appendix 
A) 

■  Compute  a  and  b 

■  If  a  >  0  AND  b  >  0 ,  obstacle  is  critical 

-  Find  Xap 

-  If  a>b,Xap=Xv  +  ri 

■  If  b>a,Xap=Xv  +  r2 

5.4:  If  Estimated  Object  is  Target: 


5.5:  Find  (X  )XY,  (^ap)xz  3  Kyy  and  E^:  Project  X  and  E  on  to  XY  and  AZ  planes 
5.6:  XFplane: 


-  Angle  error  Gy  =  cos 


v„<xj 


ap  '  XY 


\Vv 


(xj 


ap>XY 


J 


-  Desired  velocity  v  = - - - -u 

KXJ„l 

-  Sign  convention: 

■  If  v*  <  v  ,  0y  >  0 


■  If  v*  >  v ,  Gy  <  0 


Compute  control  ay 

■  DGG:  a  =  kv(v-v*) 


NGG:  a  =  kv  sin  Gy  where  k,  =  k 


yju1  +  v'  a Ju2  +  v5^ 


V  V 


5.7:  AZ  plane: 

-  Angle  error  Gz  =  cos-1 

-  Desired  velocity  iv*  = 

-  Sign  convention: 


VII  Fjz  llll  i^ap)xz  M  J 
[(Xap)Xzl 


\iXaP)xz\ 
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■  If  w*  <w ,  0_  >  0 

■  If  w*  >  w ,  0.  <  0 

-  Compute  control  a 

■  DGG:  az=kw(w-w*) 


NGG:  a„=k  sin6*_  where  k  -k 


>/ u 2  +  w2 


Vn2  +  w*2 


5.8:  State  update: 


r  - 

o 

X 

— 

~v~ 

with  U  = 

a 

V 

u 

y 

_a--_ 

5.9:  If  any  point  of  time: 

\x\<d 


That  is  if  vehicle  position  violates  obstacle  safety  boundary,  activate  Sphere 
Tracking  algorithm  given  in  Appendix  B. 
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